#lang scheme/base
;;; PLT Scheme Science Collection
;;; ode-initval.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 module provides an ordinary differential equation solver
;;; capability for PLT Scheme. 
;;;
;;; Version  Date      Description

(require "control.ss")

(provide (all-defined-out))

;(define-struct standard-control-state
;               (eps-abs
;                eps-rel
;                a_y
;                a_dydt))

(define-values (struct:standard-control-state
                standard-control-state-constructor
                standard-control-state?
                standard-control-state-field-ref
                set-standard-control-state-field!)
  (make-struct-type 'standard-control-state #f 4 0))

(define (make-standard-control-state)
  (standard-control-state-constructor 0.0 0.0 0.0 0.0))

(define standard-control-state-eps-abs
  (make-struct-field-accessor standard-control-state-field-ref 0 'eps-abs))
(define set-standard-control-state-eps-abs!
  (make-struct-field-mutator set-standard-control-state-field! 0 'eps-abs))

(define standard-control-state-eps-rel
  (make-struct-field-accessor standard-control-state-field-ref 1 'eps-rel))
(define set-standard-control-state-eps-rel!
  (make-struct-field-mutator set-standard-control-state-field! 1 'eps-rel))

(define standard-control-state-a_y
  (make-struct-field-accessor standard-control-state-field-ref 2 'a_y))
(define set-standard-control-state-a_y!
  (make-struct-field-mutator set-standard-control-state-field! 2 'a_y))

(define standard-control-state-a_dydt
  (make-struct-field-accessor standard-control-state-field-ref 3 'a_dydt))
(define set-standard-control-state-a_dydt!
  (make-struct-field-mutator set-standard-control-state-field! 3 'a_dydt))

(define (standard-control-init standard-control-state 
                               eps-abs eps-rel a_y a_dydt)
  (when (< eps-abs 0.0)
    (error 'standard-control-init "eps-abs is negative"))
  (when (< eps-rel 0.0)
    (error 'standard-control-init "eps-rel is negative"))
  (when (< a_y 0.0)
    (error 'standard-control-init "a_y is negative"))
  (when (< a_dydt 0.0)
    (error 'standard-control-init "a_dydt is negative"))
  (set-standard-control-state-eps-abs! standard-control-state eps-abs)
  (set-standard-control-state-eps-rel! standard-control-state eps-rel)
  (set-standard-control-state-a_y! standard-control-state a_y)
  (set-standard-control-state-a_dydt! standard-control-state a_dydt))

(define (standard-control-h-adjust state dim ord y y-err yp h)
  (let ((eps-abs (standard-control-state-eps-abs state))
        (eps-rel (standard-control-state-eps-rel state))
        (a_y (standard-control-state-a_y state))
        (a_dydt (standard-control-state-a_dydt state))
        (s 0.9)
        (h-old (unbox h))
        (rmax -inf.0))
    (do ((i 0 (+ i 1)))
        ((= i dim) (void))
      (let* ((D0 (+ (* eps-rel
                       (+ (* a_y (abs (vector-ref y i)))
                          (* a_dydt (abs (* h-old (vector-ref yp i))))))
                    eps-abs))
             (r (/ (abs (vector-ref y-err i)) (abs D0))))
        (set! rmax (max r rmax))))
    (let/ec exit
      (cond ((> rmax 1.1)
             ;; Decrease step, no more than a factor of 5, but a fraction
             ;; S more than scaling suggests (for better accuracy)
             (let ((r (/ s (expt rmax (/ 1.0 ord)))))
               (when (< r 0.2)
                 (set! r 0.2))
               (set-box! h (* r h-old))
               (exit -1)))
            ((< rmax 0.5)
             ;; Increase step, no more than factor of 5
             (let ((r (/ s (expt rmax (/ 1.0 (+ ord 1.0))))))
               (when (> r 5.0)
                 (set! r 5.0))
               (when (< r 1.0) ; don't allow any decrease cause by s<1
                 (set! r 1.0))
               (set-box! h (* r h-old))
               (exit 1)))
            (else
             0)))))

(define standard-control-type
  (make-ode-control-type
   "standard"
   make-standard-control-state
   standard-control-init
   standard-control-h-adjust))

(define (standard-control-new eps-abs eps-rel a_y a_dydt)
  (let ((control (make-ode-control standard-control-type)))
    (ode-control-init control eps-abs eps-rel a_y a_dydt)
    control))

(define (control-y-new eps-abs eps-rel)
  (standard-control-new eps-abs eps-rel 1.0 0.0))

(define (control-yp-new eps-abs eps-rel)
  (standard-control-new eps-abs eps-rel 0.0 1.0))