001: #include "ddeq.h"
002: 
003: /***************************************************************************/
004: /* This is a new dde solver, that attempts to get around the problem of    */
005: /* interpolating lagged variables at a lower order than the integration    */
006: /* scheme. The methods borrow heavily from:                                */
007: /* [1] C.A.H. Paul 1992 Appl.Numer.Math. 9:403-414                         */
008: /* [2] D.J. Higham 1993 Appl.Numer.Math. 12:315-330                        */
009: /* [3] Press et al. 1992 Numerical recipes in C C.U.P. chap. 16            */
010: /* The integration scheme used is an emdedded RK23 pair due to Fehlberg    */
011: /* reported in:                                                            */
012: /* [4]E.Hairer, S.P.Norsett & G.Wanner 1987, Solving Ordinary differential */
013: /*                   Equations I. springer-Verlag Berlin. p170 RKF2(3)B    */
014: /* How to derive better schemes is described in:                           */
015: /* [5] J.C. Butcher 1987 The Numerical Analysis of Ordinary Differential   */
016: /* Equations. John Wiley & sons, Chichester.                               */
017: /* Interpolation of lagged variables is by cubic hermite polynomials. This */
018: /* necessitates storing values and gradients of lagged variables. Some     */
019: /* models have to be re-cast a bit to do this, but you can always make up  */
020: /* a lagged quantity from the lagged state variables and the gradients of  */
021: /* the latter are obviously easy to find. [2] Shows why this effort is     */
022: /* required.                                                               */
023: /* Lags of less than the timestep are dealt with by extrapolating the      */
024: /* cubic polynomial for the last stored step, beyond that time step [1].   */
025: /* Switches are also dealt with. This means that lagged variables are      */
026: /* stored twice when a switch occurs: once before and once after. However, */
027: /* switch tracking is not carried out, so the step length may at times     */
028: /* reduce as switches in lagged variables are crossed, yielding derivative */
029: /* discontinuities.                                                        */
030: /* Stepping logic is from [3].                                             */
031: /* Note that the code has no scope for specifying initial histories. This  */
032: /* could be changed, but initial history problems are seriously unpleasant.*/
033: /* NOTE: code not yet optimised for minimum no. of gradient evaluations     */
034: /***************************************************************************/
035: 
036: #define ANSI
037: /*#include "inverse.h"*/
038: #include <stdlib.h>
039: #include <stdio.h>
040: #include <math.h>
041: 
042: 
043: #define ERRCON 1.89e-4
044: #define SWITCHES
045: 
046: /* The following macros are for inline calculation of cubic hermite
047:    polynomials (HERMITE) and their gradients (GHERMITE)
048:    The horrible variable names are to (hopefully) ensure that the names don't
049:    clash with anything else - find and replace with something nicer if you
050:    need to check the macro. Note this can't be translated to a language
051:    without left to right associativity.
052:    Tested 4/10/95 (HERMITE), 20/11/95 (GHERMITE) 
053:    Modified 8/1/96 - left to right evaluation sequence IS NOT standard!! */
054: 
055: 
056: #define HERMITE(res,x0,x1,y0,y1,g0,g1,x) \
057:         HeRmItE_xx0=x-x0;HeRmItE_xx1=x-x1;HeRmItE_xx12=HeRmItE_xx1*HeRmItE_xx1;\
058:         HeRmItE_xx02=HeRmItE_xx0*HeRmItE_xx0;\
059:         if (HeRmItE_h=x1-x0)\
060:   res=(( (g0)*HeRmItE_xx0*HeRmItE_xx12 + (g1)*HeRmItE_xx1*HeRmItE_xx02 \
061:   +((y0)*(2.0*HeRmItE_xx0+(HeRmItE_h))*HeRmItE_xx12- \
062:     (y1)*(2.0*HeRmItE_xx1-HeRmItE_h)*HeRmItE_xx02)/HeRmItE_h)/ \
063:     (HeRmItE_h*HeRmItE_h)); else res=y0
064: 
065: 
066: double HeRmItE_h,HeRmItE_xx1,HeRmItE_xx12,HeRmItE_xx02,HeRmItE_xx0;
067: 
068: 
069: #define GHERMITE(res,x0,x1,y0,y1,g0,g1,x) \
070:         HeRmItE_xx0=x-x0;HeRmItE_xx1=x-x1;HeRmItE_xx12=HeRmItE_xx1*HeRmItE_xx1;\
071:         HeRmItE_xx02=HeRmItE_xx0*HeRmItE_xx0;\
072: if (HeRmItE_h=x1-x0)\
073:    res=(( (g0)*(HeRmItE_xx12+2.0*HeRmItE_xx0*HeRmItE_xx1) \
074:   + (g1)*(HeRmItE_xx02+2.0*HeRmItE_xx0*HeRmItE_xx1) \
075:   + ((y0)*2.0*HeRmItE_xx1*(2.0*HeRmItE_xx0+HeRmItE_h + HeRmItE_xx1) \
076:   - (y1)*2.0*HeRmItE_xx0*(2.0*HeRmItE_xx1-HeRmItE_h + HeRmItE_xx0))/HeRmItE_h )\
077:     / (HeRmItE_h*HeRmItE_h) ); else res=g0;
078: 
079: /***************** end of definitions for hermite macros *******************/
080: 
081: 
082: /***************************************************************************/
083: /*                       Global variables                                  */
084: /***************************************************************************/
085: #include "ddeq.h"
086: //#include "dde-example.h"
087: 
088: int first=1, oldhno=-1L;
089: int oldnhv=0;
090: long accepted=0L,rejected=0L;
091: histype history;
092: /***************************************************************************/
093: /*             Routines that are not problem specific                      */
094: /***************************************************************************/
095: 
096: void initialise_dde()
097: {
098:         first = 1;
099:         oldhno = -1L;
100:         oldnhv=0;
101: }
102: 
103: void rk23(state,newstate,g,newg,error,coeff,ns,time,dt)
104: double *state,*newstate,*g,*newg,*error,*coeff,time,dt;int ns;
105: 
106: /* Takes a single integration step from time to time+dt using a 3rd order
107:    embedded Runge-Kutta Fehlberg method:
108:    E.Hairer, S.P.Norsett & G.Wanner 1987, Solving Ordinary differential
109:    Equations I. springer-Verlag Berlin. p170 RKF2(3)B
110:    The routine returns an estimated error vector for adaptive timestepping.
111:    The gradient of the state variables is to be given in function grad().
112:    The routine uses the lower order scheme for updating,
113:    fortunately Fehlberg optimised the coefficients for the lower order
114:    scheme..... 4/10/95.
115:    NOTE: not yet optimised for minimum gradient evaluations - see original
116:    table of coeffs. Partially optimised 9/10/95 Only valid for ci=b4i!
117:    Takes gradient at time in g, puts gradient at time+dt in newg - these can
118:    be the same pointer/array */
119: 
120: 
121: { static int first=1,oldns=-1;
122:   static double *k1,*k2,*k3,*k4,
123:   /* Embedded RKF table - coded this way to save addressing time */
124:   a2=0.25,  a3=27.0/40.0,
125:   b21= 0.25,
126:   b31=-189.0/800.0,  b32= 729.0/800.0,
127:   b41= 214.0/891.0,  b42= 1.0/33.0,     b43= 650.0/891.0,
128:   /* c1= 214.0/891.0,   c2= 1.0/33.0,      c3= 650.0/891.0,*/
129:   cc1= 533.0/2106.0, cc3= 800.0/1053.0, cc4=-1.0/78.0;
130:   int i;
131:   if ((first)||(oldns!=ns))
132:   { if (!first)
133:     { free(k2);free(k3);free(k4);}
134:     oldns=ns;first=0;
135:     if (ns>0)
136:     { k2=(double *)calloc(ns,sizeof(double));
137:       k3=(double *)calloc(ns,sizeof(double));
138:       k4=(double *)calloc(ns,sizeof(double));
139:     }
140:   }
141:   k1=g;
142:   for (i=0;i<ns;i++) newstate[i]=state[i]+(k1[i]*b21)*dt;
143: // Send a pointer to pastvalue to grad().
144:   grad(k2,newstate,coeff,time+dt*a2, pastvalue);
145:   for (i=0;i<ns;i++) newstate[i]=state[i]+(k1[i]*b31+k2[i]*b32)*dt;
146:   grad(k3,newstate,coeff,time+dt*a3, pastvalue);
147:   for (i=0;i<ns;i++)
148:   newstate[i]=state[i]+(k1[i]*b41+k2[i]*b42+k3[i]*b43)*dt;
149: 
150:   grad(k4,newstate,coeff,time+dt, pastvalue);
151:   for (i=0;i<ns;i++)
152:   { newg[i]=k4[i];
153:     error[i]=state[i]+(cc1*k1[i]+cc3*k3[i]+cc4*k4[i])*dt-newstate[i];
154:   }
155: }
156: 
157: 
158: void inithisbuff(nhv,histsize,nlag)
159: int nhv,nlag;long histsize;
160: 
161: /* sets up the global structure "history" and
162:    sets the global long integer history.offset to zero
163:    4/10/95, if it's been called before it clears up the old version first 23/11/95*/
164: 
165: { //static int oldnhv=0;
166:   int i;
167:   for (i=0;i<oldnhv;i++)
168:   { free(history.buff[i]);
169:     free(history.lagmarker[i]);
170:     free(history.gbuff[i]);
171:   }
172:   if (oldnhv) /* then further cleaning is required */
173:   { free(history.lagmarker);
174:     free(history.clock);
175:     free(history.buff);
176:     free(history.gbuff);
177:   }
178:   oldnhv=nhv;
179:   if (!nhv) return;
180:   history.no=(long)nhv;
181:   history.size=histsize;
182:   history.lagmarker=(long **)calloc((size_t)nhv,sizeof(long *));
183:   for (i=0;i<nhv;i++)
184:   history.lagmarker[i]=(long *)calloc((size_t)nlag,sizeof(long));
185:   history.clock=(double *)calloc((size_t)history.size,sizeof(double));
186:   history.buff=(double **)calloc((size_t)nhv,sizeof(double *));
187:   for (i=0;i<nhv;i++)
188:   history.buff[i]=(double *)calloc((size_t)history.size,sizeof(double));
189:   history.gbuff=(double **)calloc((size_t)nhv,sizeof(double *));
190:   for (i=0;i<nhv;i++)
191:   history.gbuff[i]=(double *)calloc((size_t)history.size,sizeof(double));
192:   if (!history.gbuff[nhv-1])
193:   { error("History buffer too long");
194:   }
195:   history.offset= -1L;
196: }
197: 
198: void updatehistory(g,s,c,t)
199: double *g,*s,*c,t;
200: 
201: /* updates the history record by calling the storehistory() moving the
202:    offset and updating and recording the time 4/10/95*/
203: 
204: { //static int first=1, oldhno=-1L;
205:   static double *his,*ghis;
206:   int i;
207:   if (! history.no) return;
208:   if ((first)||(oldhno!=history.no))
209:   { if (!first) { free(his);free(ghis);}
210:     first=0;his=(double *)calloc((size_t)history.no,sizeof(double));
211:     ghis=(double *)calloc((size_t)history.no,sizeof(double));
212:     oldhno=(int)history.no;
213:     history.first_time=t;
214:     history.offset=-1L;
215:   }
216:   storehistory(his,ghis,g,s,c,t);
217:   history.last_time=t;
218:   history.offset++;
219: //  printf("history.offset = %d\n", history.offset);
220:   if (history.offset==history.size) history.offset=0L;
221: //  printf("%d %f\n", history.offset, t);
222:   history.clock[history.offset]=t;
223:   for (i=0;i<history.no;i++)
224:   { history.buff[i][history.offset]=his[i];
225:     history.gbuff[i][history.offset]=ghis[i];
226:   }
227: }
228: 
229: double pastgradient(i,t,markno)
230: int i,markno;double t;
231: /* Interogates the history ringbuffers. Note that there is a fair amount of
232:    assignment of one variable to another at the start: this is to save
233:    on address calculation and speed up the routine. 4/10/95 (copy from
234:    pastvalue 20/11/95)*/
235: 
236: { long k1,k,offset,offsetplus,size;
237:   double res,*y,*g,*x,x0,x1;
238:   y=history.buff[i];g=history.gbuff[i];
239:   x=history.clock; /*local pointers improve address efficiency*/
240:   offset=history.offset;size=history.size;
241:   offsetplus=offset+1L; if (offsetplus==size) offsetplus=0L;
242:   k=history.lagmarker[i][markno];
243:   k1=k+1L;if ((k1>=size)||(k1<0L)) k1=0L;
244:   while ((x[k1]<t)&&(k1!=offset))
245:   { k1++;if (k1==size) k1=0L;}
246:   if (k1==0L) k=size-1L; else k=k1-1L;
247:   while ((x[k]>t)&&(k!=offsetplus))
248:   { if (k==0L) k=size-1L; else k--;}
249:   k1=k+1L;if (k1==size) k1=0L;
250:   if (t<x[k])
251:   {// printf("\nERROR: lag for variable %d too large at %g\n",i,history.last_time-t);
252:     error("lag too large for history buffer");
253:   }
254:   x0=x[k];x1=x[k1];
255: #ifdef SWITCHES  /* some code for not extrapolating through a switch */
256:   if ((t>x[k1])&&(x[k]==x[k1])) /* then its extrapolation through a switch */
257:   res=g[k1];     /* so use linear extrapolation just this once 20/11/95*/
258:   else
259: #endif
260:   { GHERMITE(res,x0,x1,y[k],y[k1],g[k],g[k1],t);}    /* 20/11/95*/
261:   history.lagmarker[i][markno]=k;
262:   return(res);
263: }
264: 
265: 
266: double pastvalue(i,t,markno)
267: int i,markno;double t;
268: /* Interogates the history ringbuffers. Note that there is a fair amount of
269:    assignment of one variable to another at the start: this is to save
270:    on address calculation and speed up the routine. 4/10/95*/
271: 
272: { long k1,k,offset,offsetplus,size;
273:   double res,*y,*g,*x,x0,x1;
274:   
275: //  for (i=0;i<data.no_c;i++) printf("%4d %15.6lf\n", i, data.c[i]); printf("\n");
276:   y=history.buff[i];g=history.gbuff[i];
277:   x=history.clock; /*local pointers improve address efficiency*/
278:   offset=history.offset;
279:   size=history.size;
280:   if (x[offset]==t) {
281:     return(y[offset]);
282:   }
283:   
284:   offsetplus=offset+1L; 
285:   if (offsetplus==size) offsetplus=0L;
286:   k=history.lagmarker[i][markno];
287:   k1=k+1L;
288:   if ((k1>=size)||(k1<0L)) k1=0L;
289:   
290: //  for (i=0;i<data.no_c;i++) printf("%4d %15.6lf\n", i, data.c[i]); printf("\n");
291: //  printf("While loop k1\n");
292:   while ((x[k1]<t) && (k1!=offset)) {
293: //    printf("k1 = %12ld x[k1] = %15.6lf\n", k1, x[k1]);
294:     k1++;
295:     if (k1==size) {
296: //      printf("While loop k1 = %ld --- resetting k1\n", k1);
297:       k1=0L;
298:     }
299:   }
300:   if (k1==0L) {
301:     k=size-1L; 
302:   } else {
303:     k=k1-1L;
304:   }
305: //  printf("While loop k\n");
306:   while ((x[k]>t)&&(k!=offsetplus)) { 
307: //    printf("k = %12ld\n", k);
308:     if (k==0L) k=size-1L; else k--;
309:   }
310:   k1=k+1L;
311:   if (k1==size) k1=0L;
312:   if (t<x[k])
313:   {/* printf("\nERROR: lag for variable %d too large at %g\nx[k]=%g   k=%d   t=%g\n",i,
314:              history.last_time-t,x[k],k,t);*/
315:     error("Lag too large for history buffer");
316:   }
317:   
318:   x0=x[k];x1=x[k1];
319: #ifdef SWITCHES  /* some code for not extrapolating through a switch */
320:   if ((t>x[k1])&&(x[k]==x[k1])) /* then its extrapolation through a switch */
321:   res=y[k1]+(t-x[k1])*g[k1]; /* so use linear extrapolation just this once */
322:   else
323: #endif
324:   { HERMITE(res,x0,x1,y[k],y[k1],g[k],g[k1],t);}
325: //  if (x[k1]==x[k]) return(y[k1]);
326:   /*printf("\nx0=%g   x1=%g  yk=%g  yk1=%g\n  gk=%g  gk1=%g  res=%g",
327:   x0,x1,y[k],y[k1],g[k],g[k1],res); */
328:   history.lagmarker[i][markno]=k;
329:   return(res);
330: }
331: 
332: 
333: double zeropos(x1,x2,x3,s1,s2,s3)
334: double x1,x2,x3,s1,s2,s3;
335: /* finds the root in [x1,x3] of a quadratic passing through the (xi,si)s
336:    it is assumed that s3<s1*/
337: 
338: 
339: { double z,y,zpy,a,b,c,d,a1,b1,c1,p;
340:   int ok=1;
341:   static int first=1;
342:   static double udge;
343:   if (first)
344:   { first=0;
345:     udge=1.00000001;
346:   }
347:   z=x3-x2;y=x2-x1;zpy=z+y;
348:   if (z==0.0||y==0.0) error("Error in switching: zero switch interval");
349:   a1=a=s2;c1=c=(z*s1+y*s3-zpy*s2)/(zpy*z*y);b1=b=(s2-s1)/y+c*y;
350:   d=b*b-4.0*a*c;c*=2.0;
351:   p= -a/b; /* linear only approximation - in case c numerically zero */
352:   if (c==0.0) a=p;
353:   else
354:   { if (d>=0.0)
355:     { d=sqrt(d);a=(-b+d)/c;b=(-b-d)/c;
356:       if ((b>=-y)&&(b<=z)) a=b; else
357:       if ((a<-y)||(a>z)) ok=0;
358:     }
359:     if ((d<0.0)||(!ok))
360:     { if (-s3<s1) a=z; else a=-y;}
361:     z=a1+a*b1+a*a*c1;
362:     d=a1+p*b1+p*p*c1;
363:     if (fabs(z)>fabs(d)) a=p; /* check that linear interpolation is not better */
364:   }
365:   a+=x2;
366:   if (a>x3) a=x3;
367:   if (a<=x1)
368:   { if (a==0.0) a=udge-1.0; else if (a<0.0) a/=udge; else a*=udge;}
369:   return(a);
370: }
371: 
372: 
373: 
374: double istep(sw0,newsws,s0,news,g,newg,c,err,t0,t1,nsw,ns,flickedswitch)
375: double *sw0,*newsws,*s0,*news,*g,*newg,*c,*err,t0,t1;
376: int nsw,ns,*flickedswitch;
377: 
378: /* executes RK23 step to next switch or target depending on which comes first
379:    If step is to the first switch then the number of that switch is returned
380:    in flickedswitch, but map() is not called.
381:    Returns how far it got. 5/10/95. g is assumed to contain the gradient at
382:    t0, it will contain the gradient at time t1 on exit. This improves
383:    efficiency by making use of info. calculated in the previous step of rk23.
384: */
385: 
386: { static int first=1,nsold,nswold,*flicked;
387:   static double *err1,*s1,*s2,*sw1,*sw2;
388:   int k,i,switches=0;
389:   double zp,dt,sp2,sp1,minp,udge,ds;
390:   if ((first)||(ns!=nsold)||(nsw!=nswold))
391:   { if (!first)
392:     { free(sw1);free(s1);free(sw2);free(s2);}
393:     first=0;
394:     sw1=(double *)calloc(nsw,sizeof(double));
395:     sw2=(double *)calloc(nsw,sizeof(double));
396:     s1=(double *)calloc(ns,sizeof(double));
397:     s2=(double *)calloc(ns,sizeof(double));
398:     err1=(double *)calloc(ns,sizeof(double));
399:     flicked=(int *)calloc(nsw,sizeof(int));
400:     nsold=ns;nswold=nsw;
401:   }
402:   dt=t1-t0;
403:   rk23(s0,news,g,newg,err,c,ns,t0,dt);
404:   if (nsw) switchfunctions(newsws,news,c,t1);
405:   for (i=0;i<nsw;i++)                 /* are there any switches */
406:   if ((sw0[i]>0.0)&&(newsws[i]<=0.0))
407:   { flicked[switches]=i;switches++;}
408: 
409:   if (!switches)   /* No switches so its an ordinary step */
410:   { *flickedswitch=-1;
411:     return(t1);
412:   }
413: 
414:   /* Logic for stepping to first switch */
415:   sp1=t0+dt*0.5;
416:   for (k=0;k<100;k++) /* if k gets to 100 routine fails */
417:   { rk23(s0,s1,g,newg,err,c,ns,t0,sp1-t0); /* step to approx. 1st switch position */
418:     switchfunctions(sw1,s1,c,sp1);
419: 
420:     switches=0;
421:     for (i=0;i<nsw;i++)     /* are there any switches ? MACRO after debug*/
422:     if ((sw0[i]>0.0)&&(sw1[i]<=0.0))
423:     { flicked[switches]=i;switches++;}
424: 
425:     if ((k)&&(switches==1))  /* MACRO after debug */
426:     { *flickedswitch=flicked[0];
427:       for (i=0;i<ns;i++) news[i]=s1[i];
428:       for (i=0;i<nsw;i++) newsws[i]=sw1[i];
429:       return(sp1);
430:     }
431: 
432:     rk23(s1,s2,newg,newg,err1,c,ns,sp1,t1-sp1);/* step to end of interval */
433:     switchfunctions(sw2,s2,c,t1);
434: 
435:     for (i=0;i<nsw;i++)     /* are there any switches ? MACRO after debug*/
436:     if ((sw1[i]>0.0)&&(sw2[i]<=0.0))
437:     { flicked[switches]=i;switches++;}
438: 
439:     if (!switches)  /* MACRO after debug */
440:     { *flickedswitch=-1;
441:       for (i=0;i<ns;i++)
442:       { news[i]=s2[i]; err[i]=sqrt(err[i]*err[i]+err1[i]*err1[i]);}
443:       for (i=0;i<nsw;i++) newsws[i]=sw2[i];
444:       return(t1);
445:     }
446: 
447:     /* having got this far switch positions must be estimated */
448: 
449:     /* locate the first switch */
450:     sp2=t1;minp=t1;
451:     for (i=0;i<switches;i++)
452:     { if ((t0==t1)||(sp1==t1)||(t0==sp1)) zp=t1; else
453:       zp=zeropos(t0,sp1,t1,sw0[flicked[i]],sw1[flicked[i]],sw2[flicked[i]]);
454:       if (zp<minp) { sp2=minp;minp=zp;}
455:     }
456:     sp1=minp;udge=1e-9;ds=sp2-sp1;
457:     if (ds>0.0)
458:     do { sp1+=udge*ds;udge*=10.0;}
459:     while (sp1==minp);
460:   }
461:   error("Problem with switch logic");return(t0);
462: 
463: }
464: 
465: 
466: 
467: 
468: void dde(s,c,t0,t1,dt,eps,dout,ns,nsw,nhv,hbsize,nlag,reset,fixstep, data)
469: double *s,      /* State variables */
470:        *c,      /* coefficients */
471:        t0,t1,   /* start and stop times */
472:        *dt,     /* pointer to initial timestep (returns final step - which
473:                              is step that would have been used if t1 not reached!)
474:                    max step set to 100 times this, minimum set to 1e-9 of this*/
475:        eps,     /* fractional tolerance for adaptive stepping */
476:        dout;    /* interval for output via user routine output(). Every
477:                              time a step passes 1 or more times of the form t0+i*dout
478:                              output() is called once. Hence output is only roughly
479:                              regular. dout=0.0 for no output. */
480: long hbsize;    /* The number of elements to store in the history buffer */
481: int nsw,        /* numbwer of switches */
482:     ns,         /* number of state variables */
483:     nhv,        /* number of lagged variables */
484:     reset,      /* set to 0 not to reset, to 1 to reset */
485:     nlag,       /* number of place markers per history variable */
486:     fixstep;    /* set to 0 for adaptive stepping, or 1 for fixed timestep */
487:         void *data;
488: 
489: { double D,Da,errmax,rerr,target,t,ti,
490:          *err,*newsws,*sws,*news,*newg,*dum,*sp,*nswp,*swp,*nsp,*e0,*scale;
491:   static double *g,mindt,maxdt;
492:   static int first=1;
493:   long i,iout=1L;
494:   int swi;
495: //      outputcontrol *ctrl = (outputcontrol *)data;
496:   
497:   nswp=newsws=(double *)calloc(nsw,sizeof(double));
498:   swp=sws=(double *)calloc(nsw,sizeof(double));
499:   nsp=news=(double *)calloc(ns,sizeof(double));
500:   newg=(double *)calloc(ns,sizeof(double));
501:   err=(double *)calloc(ns,sizeof(double));
502:   e0=(double *)calloc(ns,sizeof(double));
503:   scale=(double *)calloc((size_t)ns,sizeof(double));
504:   statescale(scale);
505:   if (nsw) {
506:     switchfunctions(sws,s,c,t0);
507:   }
508:   if (reset) {
509:     if (first>0) {
510:       first=0;
511:     } else {
512:       free(g);
513:     }
514:     mindt=(*dt)*1e-9;
515:     maxdt=(*dt)*100.0;
516:     g=(double *)calloc(ns,sizeof(double));
517:     inithisbuff(nhv,hbsize,nlag);
518:     grad(g,s,c,t0, pastvalue);
519:     updatehistory(g,s,c,t0);
520:   }
521:   ti=t=t0;
522:   sp=s;
523:   D= (*dt);
524:   if (dout) output(s, t0, data);
525:   while (t0<t1)
526:   { if (t0+D>t1)
527:     { target=t1;}
528:     else
529:     { target=t+D;}
530:     t=istep(sws,newsws,s,news,g,newg,c,err,t0,target,nsw,ns,&swi);
531:     errmax=0.0;
532:     if ((!fixstep)&&(D>mindt))
533:     { /* error control: see Press et al. 1992 p718*/
534:       for (i=0;i<ns;i++) /*e0[i]=eps*(fabs(s[i])+fabs((t-t0)*g[i])+1e-20);*/
535:       e0[i]=eps*(fabs(s[i])+fabs((t-t0)*(g[i]+newg[i])*0.5)+scale[i]);
536:       for (i=0;i<ns;i++)
537:       { if (err[i]<1e-150&&err[i]>-1e-150) rerr=0.0;else
538:         rerr=err[i]/e0[i];
539:         rerr=fabs(rerr);
540:         if (rerr>errmax) errmax=rerr;
541:       }
542:     }
543:     if (errmax<1.0)
544:     { accepted++;
545:       /*printf("\nt = %g    dt = %g",t,*dt);*/
546:       dum=s;s=news;news=dum;dum=sws;sws=newsws;newsws=dum;
547:       dum=g;g=newg;newg=dum;
548:       updatehistory(g,s,c,t);
549:       if (dout)   /* outputting results */
550:       if ( t > ti+iout*dout )
551:       { output(s, t, data);
552:              while (ti+iout*dout<=t) iout++;
553:       }
554:       t0=t;
555:       if ( swi > -1 )
556:       { if (dout) output(s, t, data);
557:            map(s,c,t,swi);
558:            if (dout) output(s, t, data);
559:         /* printf("\nStep at t=%g\n",t); */
560:       grad(g,s,c,t, pastvalue);
561:           updatehistory(g,s,c,t);
562:       } else   /* increase stepsize */
563:       { if ((!fixstep)&&(t<t1))
564:              D= (errmax > ERRCON ? 0.95*D*pow(errmax,-0.2) : 5.0*D);
565:         if (D>maxdt) D=maxdt;
566:       }
567:     } else
568:     { Da=t-t0; /* Step actually achieved */
569:       rejected++;
570:       /* Shrink D from Da */
571: #ifndef STEPOFF
572:       D=0.95*Da*pow(errmax,-0.25);
573:       D=( D < 0.1*Da ? 0.1*Da : D );
574:       if (D < 1e-14*(*dt))
575:       {/* printf("\nStepsize failure\n");*/}
576: #endif
577:       t=t0;
578:     }
579:   }
580:   (*dt)=D;
581:   if (dout) output(s, t1, data);
582:   for (i=0;i<ns;i++) sp[i]=s[i]; /* copying results to correct address */
583:   free(swp);free(nswp);free(nsp);free(err);free(e0);free(newg);free(scale);
584: }
585: 
586: