1 /* Scicos
2 *
3 * Copyright (C) INRIA - METALAU Project <scicos@inria.fr>
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18 *
19 * See the file ./license.txt
20 */
21 /* 11-03-2005, Masoud
22 * adding A-Jacobian
23 * istate =-1 case;
24 *
25 * xx/03/06, Alan
26 * enable matrix typed
27 * input/output regular ports
28 *
29 * xx/02/07, Alan
30 * added object paramaters/states
31 *
32 * 20/07/07, Masoud
33 * CVODE (Sundials) replaced LSODAR
34 * IDA (Sundials) replaced DASKR
35 */
36 /*--------------------------------------------------------------------------*/
37 #include <stdlib.h>
38 #include <string.h>
39 #include <wchar.h>
40 #include <math.h>
41
42 /* Sundials includes */
43 #include <cvode/cvode.h> /* prototypes for CVODES fcts. and consts. */
44 #include <cvode/cvode_dense.h> /* prototype for CVDense */
45 #include <cvode/cvode_direct.h> /* prototypes for various DlsMat operations */
46 #include <ida/ida.h>
47 #include <ida/ida_dense.h>
48 #include <ida/ida_direct.h>
49 #include <nvector/nvector_serial.h> /* serial N_Vector types, fcts., and macros */
50 #include <sundials/sundials_dense.h> /* prototypes for various DlsMat operations */
51 #include <sundials/sundials_direct.h> /* definitions of DlsMat and DENSE_ELEM */
52 #include <sundials/sundials_types.h> /* definition of type realtype */
53 #include <sundials/sundials_math.h>
54 #include <kinsol/kinsol.h>
55 #include <kinsol/kinsol_dense.h>
56 #include <kinsol/kinsol_direct.h>
57 #include <sundials/sundials_extension.h> /* uses extension for scicos */
58 #include "ida_impl.h"
59
60 #include "machine.h" /* C2F */
61 #include "scicos-def.h"
62 #include "sciprint.h"
63 #include "scicos.h"
64 #include "import.h"
65 #include "scicos_internal.h"
66 #include "blocks.h"
67 #include "core_math.h"
68 #include "storeCommand.h"
69 #include "syncexec.h"
70 #include "realtime.h"
71 #include "sci_malloc.h"
72 #include "ezxml.h"
73
74 #include "sciblk2.h"
75 #include "sciblk4.h"
76 #include "dynlib_scicos.h"
77
78 #include "configvariable_interface.h" /* getEntryPointPosition() and getEntryPointFromPosition() */
79
80 #include "lsodar.h" /* prototypes for lsodar fcts. and consts. */
81 #include "ddaskr.h" /* prototypes for ddaskr fcts. and consts. */
82
83 #if defined(linux) && defined(__i386__)
84 #include "setPrecisionFPU.h"
85 #endif
86
87 #include "localization.h"
88 #include "charEncoding.h"
89 #include "common_structure.h"
90 #include "Sciwarning.h"
91
92 /*--------------------------------------------------------------------------*/
93 typedef struct
94 {
95 void *dae_mem;
96 N_Vector ewt;
97 double *rwork;
98 int *iwork;
99 double *gwork; /* just added for a very special use: a
100 space passing to grblkdakr for zero crossing surfaces
101 when updating mode variables during initialization */
102 } *UserData;
103
104 SCICOS_IMPEXP SCSPTR_struct C2F(scsptr);
105
106 enum Solver
107 {
108 LSodar_Dynamic = 0,
109 CVode_BDF_Newton,
110 CVode_BDF_Functional,
111 CVode_Adams_Newton,
112 CVode_Adams_Functional,
113 Dormand_Prince,
114 Runge_Kutta,
115 Implicit_Runge_Kutta,
116 Crank_Nicolson,
117 IDA_BDF_Newton = 100,
118 DDaskr_BDF_Newton = 101,
119 DDaskr_BDF_GMRes = 102
120 };
121 /*--------------------------------------------------------------------------*/
122
123 #define freeall \
124 if (*neq>0) ODEFree(&ode_mem); \
125 if (*neq>0) N_VDestroy_Serial(y); \
126 if ( ng>0 ) FREE(jroot); \
127 if ( ng>0 ) FREE(zcros);
128
129
130 /* TJacque allocated by sundials */
131 #define freeallx \
132 if (*neq>0) free(TJacque); \
133 if (*neq>0) FREE(data->rwork); \
134 if (( ng>0 )&& (*neq>0)) FREE(data->gwork); \
135 if (*neq>0) N_VDestroy_Serial(data->ewt); \
136 if (*neq>0) FREE(data); \
137 if (*neq>0) DAEFree(&dae_mem); \
138 if (*neq>0) N_VDestroy_Serial(IDx); \
139 if (*neq>0) N_VDestroy_Serial(yp); \
140 if (*neq>0) N_VDestroy_Serial(yy); \
141 if ( ng>0 ) FREE(jroot); \
142 if ( ng>0 ) FREE(zcros); \
143 if (nmod>0) FREE(Mode_save);
144
145 #define freeouttbptr \
146 FREE(outtbd); \
147 FREE(outtbc); \
148 FREE(outtbs); \
149 FREE(outtbl); \
150 FREE(outtbuc); \
151 FREE(outtbus); \
152 FREE(outtbul);
153
154 #define freekinsol \
155 FREE(Mode_save); \
156 N_VDestroy_Serial(y); \
157 N_VDestroy_Serial(fscale); \
158 N_VDestroy_Serial(yscale); \
159 KINFree(&kin_mem);
160
161
162 #define ONE RCONST(1.0)
163 #define ZERO RCONST(0.0)
164 #define T0 RCONST(0.0)
165
166 // Special values for elements of 'funtyp'
167 #define FORTRAN_GATEWAY 0
168 #define UNUSED1 1
169 #define UNUSED2 2
170 #define SCIFUNC_BLOCK 3
171 #define EXPRESSION_BLOCK 5
172 #define IFTHEL_BLOCK 11
173 #define ESELECT_BLOCK 12
174 #define DEBUG_BLOCK 99
175 #define OLD_SCI_BLOCK 10005
176 /*--------------------------------------------------------------------------*/
177 /* Table of constant values */
178 static int c__90 = 90;
179 static int c__91 = 91;
180 static int c__0 = 0;
181 static double c_b14 = 0.;
182 static int c__1 = 1;
183
184 int TCritWarning = 0;
185
186 static double *t0 = NULL, *tf = NULL;
187 static double *x = NULL, *tevts = NULL, *xd = NULL, *g = NULL;
188 static double Atol = 0., rtol = 0., ttol = 0., deltat = 0., hmax = 0.;
189 static int *ierr = NULL;
190 static int *pointi = NULL;
191 static int *xptr = NULL, *modptr = NULL, *evtspt = NULL;
192 static int *funtyp = NULL, *inpptr = NULL, *outptr = NULL, *inplnk = NULL, *outlnk = NULL;
193 static int *clkptr = NULL, *ordptr = NULL, *ordclk = NULL, *cord = NULL, *iord = NULL, *oord = NULL, *zord = NULL, *critev = NULL, *zcptr = NULL;
194 static int nblk = 0, nordptr = 0, nlnk = 0, nx = 0, ng = 0, ncord = 0, noord = 0, nzord = 0;
195 static int nordclk = 0, niord = 0, nmod = 0;
196 static int nelem = 0;
197 static int *mod = NULL;
198 static int *neq = NULL;
199 static int *xprop = NULL; /* xproperty */
200 static int debug_block = 0;
201 static int *iwa = NULL;
202 static int hot;
203
204 /* declaration of ptr for typed port */
205 static void **outtbptr = NULL; /*pointer array of object of outtb*/
206 static int *outtbsz = NULL; /*size of object of outtb*/
207 static int *outtbtyp = NULL; /*type of object of outtb*/
208
209 SCSREAL_COP *outtbdptr = NULL; /*to store double of outtb*/
210 SCSINT8_COP *outtbcptr = NULL; /*to store int8 of outtb*/
211 SCSINT16_COP *outtbsptr = NULL; /*to store int16 of outtb*/
212 SCSINT32_COP *outtblptr = NULL; /*to store int32 of outtb*/
213 SCSUINT8_COP *outtbucptr = NULL; /*to store unsigned int8 of outtb */
214 SCSUINT16_COP *outtbusptr = NULL; /*to store unsigned int16 of outtb */
215 SCSUINT32_COP *outtbulptr = NULL; /*to store unsigned int32 of outtb */
216
217 static outtb_el *outtb_elem = NULL;
218
219 static scicos_block *Blocks = NULL;
220
221 /* pass to external variable for code generation */
222 /* reserved variable name */
223 int *block_error = NULL;
224 double scicos_time = 0.;
225 int phase = 0;
226 int Jacobian_Flag = 0;
227 // double CI = 0., CJ = 0.; // doubles returned by Get_Jacobian_ci and Get_Jacobian_cj respectively
228 double CJJ = 0.; // returned by Get_Jacobian_parameter
229 double SQuround = 0.;
230 /* Jacobian*/
231 static int AJacobian_block = 0;
232
233
234 /* Variable declaration moved to scicos.c because it was in the scicos-def.h therefore
235 * multiple declaration of the variable and linkers were complaining about duplicate
236 * symbols
237 */
238 SCICOS_IMPEXP COSDEBUGCOUNTER_struct C2F(cosdebugcounter);
239 SCICOS_IMPEXP RTFACTOR_struct C2F(rtfactor);
240 SCICOS_IMPEXP SOLVER_struct C2F(cmsolver);
241 SCICOS_IMPEXP CURBLK_struct C2F(curblk);
242 SCICOS_IMPEXP COSDEBUG_struct C2F(cosdebug);
243 SCICOS_IMPEXP COSHLT_struct C2F(coshlt);
244 SCICOS_IMPEXP DBCOS_struct C2F(dbcos);
245 SCICOS_IMPEXP COSTOL_struct C2F(costol);
246 SCICOS_IMPEXP COSERR_struct coserr;
247 /*--------------------------------------------------------------------------*/
248 static void FREE_blocks();
249 static void cosini(double *told);
250 static void cosend(double *told);
251 static void cossim(double *told);
252 static void cossimdaskr(double *told);
253 static void doit(double *told);
254 static void idoit(double *told);
255 static void zdoit(double *told, double *xt, double *xtd, double *g);
256 static void cdoit(double *told);
257 static void ozdoit(double *told, double *xt, double *xtd, int *kiwa);
258 static void odoit(double *told, double *xt, double *xtd, double *residual);
259 static void ddoit(double *told);
260 static void edoit(double *told, int *kiwa);
261 static void reinitdoit(double *told);
262 static int CallKinsol(double *told);
263 static int simblk(realtype t, N_Vector yy, N_Vector yp, void *f_data);
264 static int grblkdaskr(realtype t, N_Vector yy, N_Vector yp, realtype *gout, void *g_data);
265 static int grblk(realtype t, N_Vector yy, realtype *gout, void *g_data);
266 static void simblklsodar(int * nequations, realtype * tOld, realtype * actual, realtype * res);
267 static void grblklsodar(int * nequations, realtype * tOld, realtype * actual, int * ngc, realtype * res);
268 static void simblkddaskr(realtype *tOld, realtype *actual, realtype *actualP, realtype *res, int *flag, double *dummy1, int *dummy2);
269 static void grblkddaskr(int *nequations, realtype *tOld, realtype *actual, int *ngc, realtype *res, double *dummy1, int *dummy2);
270 static void jacpsol(realtype *res, int *ires, int *nequations, realtype *tOld, realtype *actual, realtype *actualP,
271 realtype *rewt, realtype *savr, realtype *wk, realtype *h, realtype *cj, realtype *wp,
272 int *iwp, int *ier, double *dummy1, int *dummy2);
273 static void psol(int *nequations, realtype *tOld, realtype *actual, realtype *actualP,
274 realtype *savr, realtype *wk, realtype *cj, realtype *wght, realtype *wp,
275 int *iwp, realtype *b, realtype *eplin, int *ier, double *dummy1, int *dummy2);
276 static void addevs(double t, int *evtnb, int *ierr1);
277 static int synchro_g_nev(ScicosImport *scs_imp, double *g, int kf, int *ierr);
278 static void Multp(double *A, double *B, double *R, int ra, int rb, int ca, int cb);
279 static int read_id(ezxml_t *elements, char *id, double *value);
280 static int simblkdaskr(realtype tres, N_Vector yy, N_Vector yp, N_Vector resval, void *rdata);
281 static void SundialsErrHandler(int error_code, const char *module, const char *function, char *msg, void *user_data);
282 static int Jacobians(long int Neq, realtype tt, realtype cj, N_Vector yy,
283 N_Vector yp, N_Vector resvec, DlsMat Jacque, void *jdata,
284 N_Vector tempv1, N_Vector tempv2, N_Vector tempv3);
285 static void call_debug_scicos(scicos_block *block, scicos_flag *flag, int flagi, int deb_blk);
286 static int synchro_nev(ScicosImport *scs_imp, int kf, int *ierr);
287 /*--------------------------------------------------------------------------*/
288 extern int C2F(dset)(int *n, double *dx, double *dy, int *incy);
289 extern int C2F(dcopy)(int *, double *, int *, double *, int *);
290 extern int C2F(dgefa)(double *A, int *lead_dim_A, int *n, int *ipivots, int *info);
291 extern int C2F(dgesl)(double *A, int *lead_dim_A, int *n, int *ipivots, double *B, int *job);
292 extern int C2F(msgs)();
293 extern void C2F(clearscicosimport)();
294 /*--------------------------------------------------------------------------*/
295 void putevs(double *t, int *evtnb, int *ierr1);
296 void Jdoit(double *told, double *xt, double *xtd, double *residual, int *job);
297 int simblkKinsol(N_Vector yy, N_Vector resval, void *rdata);
298 /*--------------------------------------------------------------------------*/
C2F(scicos)299 int C2F(scicos)(double *x_in, int *xptr_in, double *z__,
300 void **work, int *zptr, int *modptr_in,
301 void **oz, int *ozsz, int *oztyp, int *ozptr,
302 char **iz, int *izptr, char **uid, int *uidptr, double *t0_in,
303 double *tf_in, double *tevts_in, int *evtspt_in,
304 int *nevts, int *pointi_in, void **outtbptr_in,
305 int *outtbsz_in, int *outtbtyp_in,
306 outtb_el *outtb_elem_in, int *nelem1, int *nlnk1,
307 void** funptr, int *funtyp_in, int *inpptr_in,
308 int *outptr_in, int *inplnk_in, int *outlnk_in,
309 double *rpar, int *rpptr, int *ipar, int *ipptr,
310 void **opar, int *oparsz, int *opartyp, int *opptr,
311 int *clkptr_in, int *ordptr_in, int *nordptr1,
312 int *ordclk_in, int *cord_in, int *ncord1,
313 int *iord_in, int *niord1, int *oord_in,
314 int *noord1, int *zord_in, int *nzord1,
315 int *critev_in, int *nblk1, int *ztyp,
316 int *zcptr_in, int *subscr, int *nsubs,
317 double *simpar, int *flag__, int *ierr_out)
318 {
319 int i1, kf, lprt, in, out, job = 1;
320
321
322 static int mxtb = 0, ierr0 = 0, kfun0 = 0, i = 0, j = 0, k = 0, jj = 0;
323 static int ni = 0, no = 0;
324 static int nz = 0, noz = 0, nopar = 0;
325 double *W = NULL;
326
327 // Set FPU Flag to Extended for scicos simulation
328 // in order to override Java setting it to Double.
329 #if defined(linux) && defined(__i386__)
330 setFPUToExtended();
331 #endif
332 /* Copyright INRIA */
333 /* iz,izptr are used to pass block labels */
334 TCritWarning = 0;
335
336 t0 = t0_in;
337 tf = tf_in;
338 ierr = ierr_out;
339
340 /* Parameter adjustments */
341 pointi = pointi_in;
342 x = x_in;
343 xptr = xptr_in - 1;
344 modptr = modptr_in - 1;
345 --zptr;
346 //--izptr;
347 --ozptr;
348 evtspt = evtspt_in - 1;
349 tevts = tevts_in - 1;
350 outtbptr = outtbptr_in;
351 outtbsz = outtbsz_in;
352 outtbtyp = outtbtyp_in;
353 outtb_elem = outtb_elem_in;
354 funtyp = funtyp_in - 1;
355 inpptr = inpptr_in - 1;
356 outptr = outptr_in - 1;
357 inplnk = inplnk_in - 1;
358 outlnk = outlnk_in - 1;
359 --rpptr;
360 --ipptr;
361 --opptr;
362 clkptr = clkptr_in - 1;
363 ordptr = ordptr_in - 1;
364 ordclk = ordclk_in - 1;
365 cord = cord_in - 1;
366 iord = iord_in - 1;
367 oord = oord_in - 1;
368 zord = zord_in - 1;
369
370 critev = critev_in - 1;
371 --ztyp;
372 zcptr = zcptr_in - 1;
373
374 /* Function Body */
375 Atol = simpar[0];
376 rtol = simpar[1];
377 ttol = simpar[2];
378 deltat = simpar[3];
379 C2F(rtfactor).scale = simpar[4];
380 C2F(cmsolver).solver = (int) simpar[5];
381 hmax = simpar[6];
382
383 nordptr = *nordptr1;
384 nblk = *nblk1;
385 ncord = *ncord1;
386 noord = *noord1;
387 nzord = *nzord1;
388 niord = *niord1;
389 nlnk = *nlnk1;
390 nelem = *nelem1;
391 *ierr = 0;
392
393 nordclk = ordptr[nordptr] - 1; /* number of rows in ordclk is ordptr(nclkp1)-1 */
394 ng = zcptr[nblk + 1] - 1; /* computes number of zero crossing surfaces */
395 nmod = modptr[nblk + 1] - 1; /* computes number of modes */
396 nz = zptr[nblk + 1] - 1; /* number of discrete real states */
397 noz = ozptr[nblk + 1] - 1; /* number of discrete object states */
398 nopar = opptr[nblk + 1] - 1; /* number of object parameters */
399 nx = xptr[nblk + 1] - 1; /* number of object parameters */
400 neq = &nx;
401
402 xd = &x[xptr[nblk + 1] - 1];
403
404 /* check for hard coded maxsize */
405 for (i = 1; i <= nblk; ++i)
406 {
407 if (funtyp[i] < 10000)
408 {
409 funtyp[i] %= 1000;
410 }
411 else
412 {
413 funtyp[i] = funtyp[i] % 1000 + 10000;
414 }
415 ni = inpptr[i + 1] - inpptr[i];
416 no = outptr[i + 1] - outptr[i];
417 if (funtyp[i] == 1)
418 {
419 if (ni + no > 11)
420 {
421 /* hard coded maxsize in callf.c */
422 C2F(msgs)(&c__90, &c__0);
423 C2F(curblk).kfun = i;
424 *ierr = i + 1005;
425 return 0;
426 }
427 }
428 else if (funtyp[i] == 2 || funtyp[i] == 3)
429 {
430 /* hard coded maxsize in scicos.h */
431 if (ni + no > SZ_SIZE)
432 {
433 C2F(msgs)(&c__90, &c__0);
434 C2F(curblk).kfun = i;
435 *ierr = i + 1005;
436 return 0;
437 }
438 }
439 mxtb = 0;
440 if (funtyp[i] == 0)
441 {
442 if (ni > 1)
443 {
444 for (j = 1; j <= ni; ++j)
445 {
446 k = inplnk[inpptr[i] - 1 + j];
447 mxtb = mxtb + (outtbsz[k - 1] * outtbsz[(k - 1) + nlnk]);
448 }
449 }
450 if (no > 1)
451 {
452 for (j = 1; j <= no; ++j)
453 {
454 k = outlnk[outptr[i] - 1 + j];
455 mxtb = mxtb + (outtbsz[k - 1] * outtbsz[(k - 1) + nlnk]);
456 }
457 }
458 if (mxtb > TB_SIZE)
459 {
460 C2F(msgs)(&c__91, &c__0);
461 C2F(curblk).kfun = i;
462 *ierr = i + 1005;
463 return 0;
464 }
465 }
466 }
467
468 if (nx > 0) /* xprop */
469 {
470 if ((xprop = MALLOC(sizeof(int) * nx)) == NULL )
471 {
472 *ierr = 5;
473 return 0;
474 }
475 }
476 for (i = 0; i < nx; i++) /* initialize */
477 {
478 xprop[i] = 1;
479 }
480 if (nmod > 0) /* mod */
481 {
482 if ((mod = MALLOC(sizeof(int) * nmod)) == NULL )
483 {
484 *ierr = 5;
485 if (nx > 0)
486 {
487 FREE(xprop);
488 }
489 return 0;
490 }
491 }
492 if (ng > 0) /* g becomes global */
493 {
494 if ((g = MALLOC(sizeof(double) * ng)) == NULL )
495 {
496 *ierr = 5;
497 if (nmod > 0)
498 {
499 FREE(mod);
500 }
501 if (nx > 0)
502 {
503 FREE(xprop);
504 }
505 return 0;
506 }
507 }
508
509 debug_block = -1; /* no debug block for start */
510 C2F(cosdebugcounter).counter = 0;
511
512 /** Create Block's array **/
513 if ((Blocks = MALLOC(sizeof(scicos_block) * nblk)) == NULL )
514 {
515 *ierr = 5;
516 if (nx > 0)
517 {
518 FREE(xprop);
519 }
520 if (nmod > 0)
521 {
522 FREE(mod);
523 }
524 if (ng > 0)
525 {
526 FREE(g);
527 }
528 return 0;
529 }
530
531 /** Setting blocks properties for each entry in Block's array **/
532
533 /* 1 : type and pointer on simulation function */
534 for (kf = 0; kf < nblk; ++kf) /*for each block */
535 {
536 void* p = funptr[kf];
537 C2F(curblk).kfun = kf + 1;
538 Blocks[kf].type = funtyp[kf + 1];
539 if (Blocks[kf].type == IFTHEL_BLOCK)
540 {
541 funtyp[kf + 1] = -1;
542 }
543 else if (Blocks[kf].type == ESELECT_BLOCK)
544 {
545 funtyp[kf + 1] = -2;
546 }
547 else if (Blocks[kf].type < 0)
548 {
549 //macros
550 funtyp[kf + 1] *= -1; // Restore a positive 'funtyp' for later use
551 switch (-Blocks[kf].type)
552 {
553 case FORTRAN_GATEWAY:
554 Blocks[kf].funpt = (voidg)F2C(sciblk);
555 break;
556 case UNUSED1:
557 sciprint(_("type 1 function not allowed for scilab blocks\n"));
558 *ierr = 1000 + kf + 1;
559 FREE_blocks();
560 return 0;
561 case UNUSED2:
562 sciprint(_("type 2 function not allowed for scilab blocks\n"));
563 *ierr = 1000 + kf + 1;
564 FREE_blocks();
565 return 0;
566 case SCIFUNC_BLOCK:
567 Blocks[kf].funpt = (voidg)sciblk2;
568 Blocks[kf].type = 2;
569 break;
570 case EXPRESSION_BLOCK:
571 Blocks[kf].funpt = (voidg)sciblk4;
572 Blocks[kf].type = 4;
573 break;
574 case DEBUG_BLOCK: /* debugging block */
575 Blocks[kf].funpt = (voidg)sciblk4;
576 Blocks[kf].type *= -1;
577 debug_block = kf;
578 break;
579
580 case OLD_SCI_BLOCK:
581 Blocks[kf].funpt = (voidg)sciblk4;
582 Blocks[kf].type = 10004;
583 break;
584 default:
585 sciprint(_("Undefined Function type\n"));
586 *ierr = 1000 + kf + 1;
587 FREE_blocks();
588 return 0;
589 }
590 Blocks[kf].scsptr = p; /* set scilab function adress for sciblk */
591 }
592 else
593 {
594 //linked functions (internal or external)
595 Blocks[kf].funpt = (voidf)p;
596 Blocks[kf].scsptr = NULL; /* this is done for being able to test if a block
597 is a scilab block in the debugging phase when
598 sciblk4 is called */
599 }
600
601 /* 2 : Dimension properties */
602 Blocks[kf].ztyp = ztyp[kf + 1];
603 Blocks[kf].nx = xptr[kf + 2] - xptr[kf + 1]; /* continuous state dimension*/
604 Blocks[kf].ng = zcptr[kf + 2] - zcptr[kf + 1]; /* number of zero crossing surface*/
605 Blocks[kf].nz = zptr[kf + 2] - zptr[kf + 1]; /* number of double discrete state*/
606 Blocks[kf].noz = ozptr[kf + 2] - ozptr[kf + 1]; /* number of other discrete state*/
607 Blocks[kf].nrpar = rpptr[kf + 2] - rpptr[kf + 1]; /* size of double precision parameter vector*/
608 Blocks[kf].nipar = ipptr[kf + 2] - ipptr[kf + 1]; /* size of integer precision parameter vector*/
609 Blocks[kf].nopar = opptr[kf + 2] - opptr[kf + 1]; /* number of other parameters (matrix, data structure,..)*/
610 Blocks[kf].nin = inpptr[kf + 2] - inpptr[kf + 1]; /* number of input ports */
611 Blocks[kf].nout = outptr[kf + 2] - outptr[kf + 1]; /* number of output ports */
612
613 /* 3 : input port properties */
614 /* in insz, we store :
615 * - insz[0..nin-1] : first dimension of input ports
616 * - insz[nin..2*nin-1] : second dimension of input ports
617 * - insz[2*nin..3*nin-1] : type of data of input ports
618 */
619 /* allocate size and pointer arrays (number of input ports)*/
620 Blocks[kf].insz = NULL;
621 Blocks[kf].inptr = NULL;
622 if (Blocks[kf].nin != 0)
623 {
624 if ((Blocks[kf].insz = MALLOC(Blocks[kf].nin * 3 * sizeof(int))) == NULL )
625 {
626 FREE_blocks();
627 *ierr = 5;
628 return 0;
629 }
630 if ((Blocks[kf].inptr = MALLOC(Blocks[kf].nin * sizeof(double*))) == NULL )
631 {
632 FREE_blocks();
633 *ierr = 5;
634 return 0;
635 }
636 }
637 for (in = 0; in < Blocks[kf].nin; in++)
638 {
639 lprt = inplnk[inpptr[kf + 1] + in];
640 Blocks[kf].inptr[in] = outtbptr[lprt - 1]; /* pointer on the data*/
641 Blocks[kf].insz[in] = outtbsz[lprt - 1]; /* row dimension of the input port*/
642 Blocks[kf].insz[Blocks[kf].nin + in] = outtbsz[(lprt - 1) + nlnk]; /* column dimension of the input port*/
643 Blocks[kf].insz[2 * Blocks[kf].nin + in] = outtbtyp[lprt - 1]; /*type of data of the input port*/
644 }
645 /* 4 : output port properties */
646 /* in outsz, we store :
647 * - outsz[0..nout-1] : first dimension of output ports
648 * - outsz[nout..2*nout-1] : second dimension of output ports
649 * - outsz[2*nout..3*nout-1] : type of data of output ports
650 */
651 /* allocate size and pointer arrays (number of output ports)*/
652 Blocks[kf].outsz = NULL;
653 Blocks[kf].outptr = NULL;
654 if (Blocks[kf].nout != 0)
655 {
656 if ((Blocks[kf].outsz = MALLOC(Blocks[kf].nout * 3 * sizeof(int))) == NULL )
657 {
658 FREE_blocks();
659 *ierr = 5;
660 return 0;
661 }
662 if ((Blocks[kf].outptr = MALLOC(Blocks[kf].nout * sizeof(double*))) == NULL )
663 {
664 FREE_blocks();
665 *ierr = 5;
666 return 0;
667 }
668 }
669 /* set the values */
670 for (out = 0; out < Blocks[kf].nout; out++) /*for each output port */
671 {
672 lprt = outlnk[outptr[kf + 1] + out];
673 Blocks[kf].outptr[out] = outtbptr[lprt - 1]; /*pointer on data */
674 Blocks[kf].outsz[out] = outtbsz[lprt - 1]; /*row dimension of output port*/
675 Blocks[kf].outsz[Blocks[kf].nout + out] = outtbsz[(lprt - 1) + nlnk]; /*column dimension of output ports*/
676 Blocks[kf].outsz[2 * Blocks[kf].nout + out] = outtbtyp[lprt - 1]; /*type of data of output port */
677 }
678
679 /* 5 : event output port properties */
680 Blocks[kf].evout = NULL;
681 Blocks[kf].nevout = clkptr[kf + 2] - clkptr[kf + 1];
682 if (Blocks[kf].nevout != 0)
683 {
684 if ((Blocks[kf].evout = CALLOC(Blocks[kf].nevout, sizeof(double))) == NULL )
685 {
686 FREE_blocks();
687 *ierr = 5;
688 return 0;
689 }
690 }
691
692 /* 6 : pointer on the begining of the double discrete state array ( z) */
693 Blocks[kf].z = &(z__[zptr[kf + 1] - 1]);
694
695 /* 7 : type, size and pointer on the other discrete states data structures (oz) */
696 Blocks[kf].ozsz = NULL;
697 if (Blocks[kf].noz == 0)
698 {
699 Blocks[kf].ozptr = NULL;
700 Blocks[kf].oztyp = NULL;
701 }
702 else
703 {
704 Blocks[kf].ozptr = &(oz[ozptr[kf + 1] - 1]);
705 if ((Blocks[kf].ozsz = MALLOC(Blocks[kf].noz * 2 * sizeof(int))) == NULL )
706 {
707 FREE_blocks();
708 *ierr = 5;
709 return 0;
710 }
711 for (i = 0; i < Blocks[kf].noz; i++)
712 {
713 Blocks[kf].ozsz[i] = ozsz[(ozptr[kf + 1] - 1) + i];
714 Blocks[kf].ozsz[i + Blocks[kf].noz] = ozsz[(ozptr[kf + 1] - 1 + noz) + i];
715 }
716 Blocks[kf].oztyp = &(oztyp[ozptr[kf + 1] - 1]);
717 }
718
719 /* 8 : pointer on the begining of the double parameter array ( rpar ) */
720 Blocks[kf].rpar = &(rpar[rpptr[kf + 1] - 1]);
721
722 /* 9 : pointer on the begining of the integer parameter array ( ipar ) */
723 Blocks[kf].ipar = &(ipar[ipptr[kf + 1] - 1]);
724
725 /* 10 : type, size and pointer on the other parameters data structures (opar) */
726 Blocks[kf].oparsz = NULL;
727 if (Blocks[kf].nopar == 0)
728 {
729 Blocks[kf].oparptr = NULL;
730 Blocks[kf].opartyp = NULL;
731 }
732 else
733 {
734 Blocks[kf].oparptr = &(opar[opptr[kf + 1] - 1]);
735 if ((Blocks[kf].oparsz = MALLOC(Blocks[kf].nopar * 2 * sizeof(int))) == NULL)
736 {
737 FREE_blocks();
738 *ierr = 5;
739 return 0;
740 }
741 for (i = 0; i < Blocks[kf].nopar; i++)
742 {
743 Blocks[kf].oparsz[i] = oparsz[(opptr[kf + 1] - 1) + i];
744 Blocks[kf].oparsz[i + Blocks[kf].nopar] = oparsz[(opptr[kf + 1] - 1 + nopar) + i];
745 }
746 Blocks[kf].opartyp = &(opartyp[opptr[kf + 1] - 1]);
747 }
748
749 /* 10 : pointer on the beginning of the residual array (res) */
750 Blocks[kf].res = NULL;
751 if (Blocks[kf].nx != 0)
752 {
753 if ((Blocks[kf].res = MALLOC(Blocks[kf].nx * sizeof(double))) == NULL)
754 {
755 FREE_blocks();
756 *ierr = 5;
757 return 0;
758 }
759 }
760
761 /* 11 : block label (label) */
762 i1 = izptr[kf];
763 if ((Blocks[kf].label = MALLOC(sizeof(char) * (i1 + 1))) == NULL)
764 {
765 FREE_blocks();
766 *ierr = 5;
767 return 0;
768 }
769 Blocks[kf].label[i1] = '\0';
770 strcpy(Blocks[kf].label, iz[kf]);
771
772 /* block uid (uid) */
773 i1 = uidptr[kf];
774 if ((Blocks[kf].uid = MALLOC(sizeof(char) * (i1 + 1))) == NULL)
775 {
776 FREE_blocks();
777 *ierr = 5;
778 return 0;
779 }
780 Blocks[kf].uid[i1] = '\0';
781 strcpy(Blocks[kf].uid, uid[kf]);
782
783 /* 12 : block array of crossed surfaces (jroot) */
784 Blocks[kf].jroot = NULL;
785 if (Blocks[kf].ng > 0)
786 {
787 if ((Blocks[kf].jroot = CALLOC(Blocks[kf].ng, sizeof(int))) == NULL)
788 {
789 FREE_blocks();
790 *ierr = 5;
791 return 0;
792 }
793 }
794
795 /* 13 : block work array (work) */
796 Blocks[kf].work = (void **)(((double *)work) + kf);
797
798 /* 14 : block modes array (mode) */
799 Blocks[kf].nmode = modptr[kf + 2] - modptr[kf + 1];
800 if (Blocks[kf].nmode != 0)
801 {
802 Blocks[kf].mode = &(mod[modptr[kf + 1] - 1]);
803 }
804
805 /* 15 : block xprop array (xprop) */
806 Blocks[kf].xprop = NULL;
807 if (Blocks[kf].nx != 0)
808 {
809 Blocks[kf].xprop = &(xprop[xptr[kf + 1] - 1]);
810 }
811
812 /* 16 : pointer on the zero crossing surface computation function of the block (g) */
813 Blocks[kf].g = NULL;
814 if (Blocks[kf].ng != 0)
815 {
816 Blocks[kf].g = &(g[zcptr[kf + 1] - 1]);
817 }
818 }
819 /** all block properties are stored in the Blocks array **/
820
821 /* iwa */
822 iwa = NULL;
823 if ((*nevts) != 0)
824 {
825 if ((iwa = MALLOC(sizeof(int) * (*nevts))) == NULL)
826 {
827 FREE_blocks();
828 *ierr = 5;
829 return 0;
830 }
831 }
832
833 /* save ptr of scicos in import structure */
834 makescicosimport(x, &nx, &xptr[1], &zcptr[1], z__, &nz, &zptr[1],
835 &noz, oz, ozsz, oztyp, &ozptr[1],
836 g, &ng, mod, &nmod, &modptr[1], iz, &izptr[1],
837 uid, uidptr,
838 &inpptr[1], &inplnk[1], &outptr[1], &outlnk[1],
839 outtbptr, outtbsz, outtbtyp,
840 outtb_elem, &nelem,
841 &nlnk, rpar, &rpptr[1], ipar, &ipptr[1],
842 opar, oparsz, opartyp, &opptr[1],
843 &nblk, subscr, nsubs,
844 &tevts[1], &evtspt[1], nevts, pointi,
845 &iord[1], &niord, &oord[1], &noord, &zord[1], &nzord,
846 funptr, &funtyp[1], &ztyp[1],
847 &cord[1], &ncord, &ordclk[1], &nordclk, &clkptr[1],
848 &ordptr[1], &nordptr, &critev[1], iwa, Blocks,
849 t0, tf, &Atol, &rtol, &ttol, &deltat, &hmax,
850 xprop, xd);
851
852 if (*flag__ == 1) /*start*/
853 {
854 /* blocks initialization */
855 for (kf = 0; kf < nblk; ++kf)
856 {
857 *(Blocks[kf].work) = NULL;
858 }
859 cosini(t0);
860 if (*ierr != 0)
861 {
862 ierr0 = *ierr;
863 kfun0 = C2F(curblk).kfun;
864 cosend(t0);
865 *ierr = ierr0;
866 C2F(curblk).kfun = kfun0;
867 }
868
869 }
870 else if (*flag__ == 2) /*run*/
871 {
872
873 /* integration */
874 switch (C2F(cmsolver).solver)
875 {
876 case LSodar_Dynamic:
877 case CVode_BDF_Newton:
878 case CVode_BDF_Functional:
879 case CVode_Adams_Newton:
880 case CVode_Adams_Functional:
881 case Dormand_Prince:
882 case Runge_Kutta:
883 case Implicit_Runge_Kutta:
884 case Crank_Nicolson:
885 cossim(t0);
886 break;
887 case IDA_BDF_Newton:
888 case DDaskr_BDF_Newton:
889 case DDaskr_BDF_GMRes:
890 cossimdaskr(t0);
891 break;
892 default: // Unknown solver number
893 *ierr = 1000;
894 return 0;
895 }
896 if (*ierr != 0)
897 {
898 ierr0 = *ierr;
899 kfun0 = C2F(curblk).kfun;
900 cosend(t0);
901 *ierr = ierr0;
902 C2F(curblk).kfun = kfun0;
903 }
904
905 }
906 else if (*flag__ == 3) /*finish*/
907 {
908 /* blocks closing */
909 cosend(t0);
910 }
911 else if (*flag__ == 4) /*linear*/
912 {
913 phase = 1;
914 idoit(t0);
915 if (*ierr == 0 && Max(nx, ng) > 0)
916 {
917 if ((W = MALLOC(sizeof(double) * (Max(nx, ng)))) == NULL )
918 {
919 FREE(iwa);
920 FREE_blocks();
921 *ierr = 5;
922 return 0;
923 }
924
925 /*---------instead of old simblk--------*/
926 /* C2F(simblk)(&nx, t0, x, W); */
927
928 if (ng > 0 && nmod > 0)
929 {
930 zdoit(t0, x, x + nx, W); /* updating modes as a function of state values; this was necessary in iGUI*/
931 }
932 for (jj = 0; jj < nx; jj++)
933 {
934 W[jj] = 0.0;
935 }
936 C2F(ierode).iero = 0;
937 *ierr = 0;
938 if (C2F(cmsolver).solver < 100)
939 {
940 odoit(t0, x, W, W);
941 }
942 else
943 {
944 odoit(t0, x, x + nx, W);
945 }
946 C2F(ierode).iero = *ierr;
947 /*-----------------------------------------*/
948 for (i = 0; i < nx; ++i)
949 {
950 x[i] = W[i];
951 }
952 FREE(W);
953 }
954 }
955 else if (*flag__ == 5) /* initial_KINSOL= "Kinsol" */
956 {
957 C2F(ierode).iero = 0;
958 *ierr = 0;
959 idoit(t0);
960 CallKinsol(t0);
961 *ierr = C2F(ierode).iero;
962 }
963
964
965 FREE(iwa);
966 FREE_blocks();
967
968 C2F(clearscicosimport)();
969 return 0;
970 } /* scicos_ */
971 /*--------------------------------------------------------------------------*/
972 /* check_flag */
check_flag(void * flagvalue,char * funcname,int opt)973 static int check_flag(void *flagvalue, char *funcname, int opt)
974 {
975 int *errflag = NULL;
976
977 /* Check if SUNDIALS function returned NULL pointer - no memory allocated */
978 if (opt == 0 && flagvalue == NULL)
979 {
980 sciprint(_("\nSUNDIALS_ERROR: %s() failed - returned NULL pointer\n\n"), funcname);
981 return (1);
982 }
983 /* Check if flag < 0 */
984 else if (opt == 1)
985 {
986 errflag = (int *) flagvalue;
987 if (*errflag < 0)
988 {
989 sciprint(_("\nSUNDIALS_ERROR: %s() failed with flag = %d\n\n"),
990 funcname, *errflag);
991 return (1);
992 }
993 }
994 /* Check if function returned NULL pointer - no memory allocated */
995 else if (opt == 2 && flagvalue == NULL)
996 {
997 sciprint(_("\nMEMORY_ERROR: %s() failed - returned NULL pointer\n\n"), funcname);
998 return (1);
999 }
1000
1001 return (0);
1002 } /* check_flag */
1003
1004 /*--------------------------------------------------------------------------*/
cosini(double * told)1005 static void cosini(double *told)
1006 {
1007 static scicos_flag flag__ = 0;
1008 static int i = 0;
1009
1010 static int kfune = 0;
1011 static int jj = 0;
1012
1013 SCSREAL_COP *outtbd = NULL; /*to save double of outtb*/
1014 SCSINT8_COP *outtbc = NULL; /*to save int8 of outtb*/
1015 SCSINT16_COP *outtbs = NULL; /*to save int16 of outtb*/
1016 SCSINT32_COP *outtbl = NULL; /*to save int32 of outtb*/
1017 SCSUINT8_COP *outtbuc = NULL; /*to save unsigned int8 of outtb*/
1018 SCSUINT16_COP *outtbus = NULL; /*to save unsigned int16 of outtb*/
1019 SCSUINT32_COP *outtbul = NULL; /*to save unsigned int32 of outtb*/
1020 int szouttbd = 0; /*size of arrays*/
1021 int szouttbc = 0, szouttbs = 0, szouttbl = 0;
1022 int szouttbuc = 0, szouttbus = 0, szouttbul = 0;
1023 int curouttbd = 0; /*current position in arrays*/
1024 int curouttbc = 0, curouttbs = 0, curouttbl = 0;
1025 int curouttbuc = 0, curouttbus = 0, curouttbul = 0;
1026
1027 int ii = 0, kk = 0; /*local counters*/
1028 int sszz = 0; /*local size of element of outtb*/
1029 /*Allocation of arrays for outtb*/
1030 for (ii = 0; ii < nlnk; ii++)
1031 {
1032 switch (outtbtyp[ii])
1033 {
1034 case SCSREAL_N :
1035 szouttbd += outtbsz[ii] * outtbsz[ii + nlnk]; /*double real matrix*/
1036 outtbd = (SCSREAL_COP *) REALLOC (outtbd, szouttbd * sizeof(SCSREAL_COP));
1037 memset(outtbd, 0, szouttbd * sizeof(SCSREAL_COP));
1038 break;
1039
1040 case SCSCOMPLEX_N :
1041 szouttbd += 2 * outtbsz[ii] * outtbsz[ii + nlnk]; /*double complex matrix*/
1042 outtbd = (SCSCOMPLEX_COP *) REALLOC (outtbd, szouttbd * sizeof(SCSCOMPLEX_COP));
1043 memset(outtbd, 0, szouttbd * sizeof(SCSCOMPLEX_COP));
1044 break;
1045
1046 case SCSINT8_N :
1047 szouttbc += outtbsz[ii] * outtbsz[ii + nlnk]; /*int8*/
1048 outtbc = (SCSINT8_COP *) REALLOC (outtbc, szouttbc * sizeof(SCSINT8_COP));
1049 memset(outtbc, 0, szouttbc * sizeof(SCSINT8_COP));
1050 break;
1051
1052 case SCSINT16_N :
1053 szouttbs += outtbsz[ii] * outtbsz[ii + nlnk]; /*int16*/
1054 outtbs = (SCSINT16_COP *) REALLOC (outtbs, szouttbs * sizeof(SCSINT16_COP));
1055 memset(outtbs, 0, szouttbs * sizeof(SCSINT16_COP));
1056 break;
1057
1058 case SCSINT32_N :
1059 szouttbl += outtbsz[ii] * outtbsz[ii + nlnk]; /*int32*/
1060 outtbl = (SCSINT32_COP *) REALLOC (outtbl, szouttbl * sizeof(SCSINT32_COP));
1061 memset(outtbl, 0, szouttbl * sizeof(SCSINT32_COP));
1062 break;
1063
1064 case SCSUINT8_N :
1065 szouttbuc += outtbsz[ii] * outtbsz[ii + nlnk]; /*uint8*/
1066 outtbuc = (SCSUINT8_COP *) REALLOC (outtbuc, szouttbuc * sizeof(SCSUINT8_COP));
1067 memset(outtbuc, 0, szouttbuc * sizeof(SCSUINT8_COP));
1068 break;
1069
1070 case SCSUINT16_N :
1071 szouttbus += outtbsz[ii] * outtbsz[ii + nlnk]; /*uint16*/
1072 outtbus = (SCSUINT16_COP *) REALLOC (outtbus, szouttbus * sizeof(SCSUINT16_COP));
1073 memset(outtbus, 0, szouttbus * sizeof(SCSUINT16_COP));
1074 break;
1075
1076 case SCSUINT32_N :
1077 szouttbul += outtbsz[ii] * outtbsz[ii + nlnk]; /*uint32*/
1078 outtbul = (SCSUINT32_COP *) REALLOC (outtbul, szouttbul * sizeof(SCSUINT32_COP));
1079 memset(outtbul, 0, szouttbul * sizeof(SCSUINT32_COP));
1080 break;
1081
1082 default : /* Add a message here */
1083 break;
1084 }
1085 }
1086
1087 /* Jacobian*/
1088 AJacobian_block = 0;
1089
1090 /* Function Body */
1091 *ierr = 0;
1092
1093 /* initialization (flag 4) */
1094 /* loop on blocks */
1095 C2F(dset)(&ng, &c_b14, g, &c__1);
1096
1097 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun)
1098 {
1099 flag__ = 4;
1100 if (Blocks[C2F(curblk).kfun - 1].nx > 0)
1101 {
1102 Blocks[C2F(curblk).kfun - 1].x = &x[xptr[C2F(curblk).kfun] - 1];
1103 Blocks[C2F(curblk).kfun - 1].xd = &xd[xptr[C2F(curblk).kfun] - 1];
1104 }
1105 Blocks[C2F(curblk).kfun - 1].nevprt = 0;
1106 if (funtyp[C2F(curblk).kfun] >= 0) /* debug_block is not called here */
1107 {
1108 /*callf(told, xd, x, x,g,&flag__);*/
1109 Jacobian_Flag = 0;
1110 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
1111 if (flag__ < 0 && *ierr == 0)
1112 {
1113 *ierr = 5 - flag__;
1114 kfune = C2F(curblk).kfun;
1115 }
1116 if ((Jacobian_Flag == 1) && (AJacobian_block == 0))
1117 {
1118 AJacobian_block = C2F(curblk).kfun;
1119 }
1120 }
1121 }
1122 if (*ierr != 0)
1123 {
1124 C2F(curblk).kfun = kfune;
1125 freeouttbptr;
1126 return;
1127 }
1128
1129 /* initialization (flag 6) */
1130 flag__ = 6;
1131 for (jj = 1; jj <= ncord; ++jj)
1132 {
1133 C2F(curblk).kfun = cord[jj];
1134 Blocks[C2F(curblk).kfun - 1].nevprt = 0;
1135 if (funtyp[C2F(curblk).kfun] >= 0)
1136 {
1137 /*callf(told, xd, x, x,g,&flag__);*/
1138 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
1139 if (flag__ < 0)
1140 {
1141 *ierr = 5 - flag__;
1142 freeouttbptr;
1143 return;
1144 }
1145 }
1146 }
1147
1148 /* point-fix iterations */
1149 flag__ = 6;
1150 for (i = 1; i <= nblk + 1; ++i) /*for each block*/
1151 {
1152 /* loop on blocks */
1153 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun)
1154 {
1155 Blocks[C2F(curblk).kfun - 1].nevprt = 0;
1156 if (funtyp[C2F(curblk).kfun] >= 0)
1157 {
1158 /*callf(told, xd, x, x,g,&flag__);*/
1159 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
1160 if (flag__ < 0)
1161 {
1162 *ierr = 5 - flag__;
1163 freeouttbptr;
1164 return;
1165 }
1166 }
1167 }
1168
1169 flag__ = 6;
1170 for (jj = 1; jj <= ncord; ++jj) /*for each continuous block*/
1171 {
1172 C2F(curblk).kfun = cord[jj];
1173 if (funtyp[C2F(curblk).kfun] >= 0)
1174 {
1175 /*callf(told, xd, x, x,g,&flag__);*/
1176 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
1177 if (flag__ < 0)
1178 {
1179 *ierr = 5 - flag__;
1180 freeouttbptr;
1181 return;
1182 }
1183 }
1184 }
1185
1186 /*comparison between outtb and arrays*/
1187 curouttbd = 0;
1188 curouttbc = 0;
1189 curouttbs = 0;
1190 curouttbl = 0;
1191 curouttbuc = 0;
1192 curouttbus = 0;
1193 curouttbul = 0;
1194 for (jj = 0; jj < nlnk; jj++)
1195 {
1196 switch (outtbtyp[jj]) /*for each type of ports*/
1197 {
1198 case SCSREAL_N :
1199 outtbdptr = (SCSREAL_COP *)outtbptr[jj]; /*double real matrix*/
1200 sszz = outtbsz[jj] * outtbsz[jj + nlnk];
1201 for (kk = 0; kk < sszz; kk++)
1202 {
1203 int outtbdptr_isnan = outtbdptr[kk] != outtbdptr[kk];
1204 int outtbd_isnan = (SCSREAL_COP)outtbd[curouttbd + kk] != (SCSREAL_COP)outtbd[curouttbd + kk];
1205
1206 if (outtbdptr_isnan && outtbd_isnan)
1207 {
1208 continue;
1209 }
1210 if (outtbdptr[kk] != (SCSREAL_COP)outtbd[curouttbd + kk])
1211 {
1212 goto L30;
1213 }
1214 }
1215 curouttbd += sszz;
1216 break;
1217
1218 case SCSCOMPLEX_N :
1219 outtbdptr = (SCSCOMPLEX_COP *)outtbptr[jj]; /*double complex matrix*/
1220 sszz = 2 * outtbsz[jj] * outtbsz[jj + nlnk];
1221 for (kk = 0; kk < sszz; kk++)
1222 {
1223 int outtbdptr_isnan = outtbdptr[kk] != outtbdptr[kk];
1224 int outtbd_isnan = (SCSCOMPLEX_COP)outtbd[curouttbd + kk] != (SCSCOMPLEX_COP)outtbd[curouttbd + kk];
1225
1226 if (outtbdptr_isnan && outtbd_isnan)
1227 {
1228 continue;
1229 }
1230 if (outtbdptr[kk] != (SCSCOMPLEX_COP)outtbd[curouttbd + kk])
1231 {
1232 goto L30;
1233 }
1234 }
1235 curouttbd += sszz;
1236 break;
1237
1238 case SCSINT8_N :
1239 outtbcptr = (SCSINT8_COP *)outtbptr[jj]; /*int8*/
1240 sszz = outtbsz[jj] * outtbsz[jj + nlnk];
1241 for (kk = 0; kk < sszz; kk++)
1242 {
1243 if (outtbcptr[kk] != (SCSINT8_COP)outtbc[curouttbc + kk])
1244 {
1245 goto L30;
1246 }
1247 }
1248 curouttbc += sszz;
1249 break;
1250
1251 case SCSINT16_N :
1252 outtbsptr = (SCSINT16_COP *)outtbptr[jj]; /*int16*/
1253 sszz = outtbsz[jj] * outtbsz[jj + nlnk];
1254 for (kk = 0; kk < sszz; kk++)
1255 {
1256 if (outtbsptr[kk] != (SCSINT16_COP)outtbs[curouttbs + kk])
1257 {
1258 goto L30;
1259 }
1260 }
1261 curouttbs += sszz;
1262 break;
1263
1264 case SCSINT32_N :
1265 outtblptr = (SCSINT32_COP *)outtbptr[jj]; /*int32*/
1266 sszz = outtbsz[jj] * outtbsz[jj + nlnk];
1267 for (kk = 0; kk < sszz; kk++)
1268 {
1269 if (outtblptr[kk] != (SCSINT32_COP)outtbl[curouttbl + kk])
1270 {
1271 goto L30;
1272 }
1273 }
1274 curouttbl += sszz;
1275 break;
1276
1277 case SCSUINT8_N :
1278 outtbucptr = (SCSUINT8_COP *)outtbptr[jj]; /*uint8*/
1279 sszz = outtbsz[jj] * outtbsz[jj + nlnk];
1280 for (kk = 0; kk < sszz; kk++)
1281 {
1282 if (outtbucptr[kk] != (SCSUINT8_COP)outtbuc[curouttbuc + kk])
1283 {
1284 goto L30;
1285 }
1286 }
1287 curouttbuc += sszz;
1288 break;
1289
1290 case SCSUINT16_N :
1291 outtbusptr = (SCSUINT16_COP *)outtbptr[jj]; /*uint16*/
1292 sszz = outtbsz[jj] * outtbsz[jj + nlnk];
1293 for (kk = 0; kk < sszz; kk++)
1294 {
1295 if (outtbusptr[kk] != (SCSUINT16_COP)outtbus[curouttbus + kk])
1296 {
1297 goto L30;
1298 }
1299 }
1300 curouttbus += sszz;
1301 break;
1302
1303 case SCSUINT32_N :
1304 outtbulptr = (SCSUINT32_COP *)outtbptr[jj]; /*uint32*/
1305 sszz = outtbsz[jj] * outtbsz[jj + nlnk];
1306 for (kk = 0; kk < sszz; kk++)
1307 {
1308 if (outtbulptr[kk] != (SCSUINT32_COP)outtbul[curouttbul + kk])
1309 {
1310 goto L30;
1311 }
1312 }
1313 curouttbul += sszz;
1314 break;
1315
1316 default : /* Add a message here */
1317 break;
1318 }
1319 }
1320 freeouttbptr;
1321 return;
1322
1323 L30:
1324 /*Save data of outtb in arrays*/
1325 curouttbd = 0;
1326 curouttbc = 0;
1327 curouttbs = 0;
1328 curouttbl = 0;
1329 curouttbuc = 0;
1330 curouttbus = 0;
1331 curouttbul = 0;
1332 for (ii = 0; ii < nlnk; ii++) /*for each link*/
1333 {
1334 switch (outtbtyp[ii]) /*switch to type of outtb object*/
1335 {
1336 case SCSREAL_N :
1337 outtbdptr = (SCSREAL_COP *)outtbptr[ii]; /*double real matrix*/
1338 sszz = outtbsz[ii] * outtbsz[ii + nlnk];
1339 C2F(dcopy)(&sszz, outtbdptr, &c__1, &outtbd[curouttbd], &c__1);
1340 curouttbd += sszz;
1341 break;
1342
1343 case SCSCOMPLEX_N :
1344 outtbdptr = (SCSCOMPLEX_COP *)outtbptr[ii]; /*double complex matrix*/
1345 sszz = 2 * outtbsz[ii] * outtbsz[ii + nlnk];
1346 C2F(dcopy)(&sszz, outtbdptr, &c__1, &outtbd[curouttbd], &c__1);
1347 curouttbd += sszz;
1348 break;
1349
1350 case SCSINT8_N :
1351 outtbcptr = (SCSINT8_COP *)outtbptr[ii]; /*int8*/
1352 sszz = outtbsz[ii] * outtbsz[ii + nlnk];
1353 for (kk = 0; kk < sszz; kk++)
1354 {
1355 outtbc[curouttbc + kk] = (SCSINT8_COP)outtbcptr[kk];
1356 }
1357 curouttbc += sszz;
1358 break;
1359
1360 case SCSINT16_N :
1361 outtbsptr = (SCSINT16_COP *)outtbptr[ii]; /*int16*/
1362 sszz = outtbsz[ii] * outtbsz[ii + nlnk];
1363 for (kk = 0; kk < sszz; kk++)
1364 {
1365 outtbs[curouttbs + kk] = (SCSINT16_COP)outtbsptr[kk];
1366 }
1367 curouttbs += sszz;
1368 break;
1369
1370 case SCSINT32_N :
1371 outtblptr = (SCSINT32_COP *)outtbptr[ii]; /*int32*/
1372 sszz = outtbsz[ii] * outtbsz[ii + nlnk];
1373 for (kk = 0; kk < sszz; kk++)
1374 {
1375 outtbl[curouttbl + kk] = (SCSINT32_COP)outtblptr[kk];
1376 }
1377 curouttbl += sszz;
1378 break;
1379
1380 case SCSUINT8_N :
1381 outtbucptr = (SCSUINT8_COP *)outtbptr[ii]; /*uint8*/
1382 sszz = outtbsz[ii] * outtbsz[ii + nlnk];
1383 for (kk = 0; kk < sszz; kk++)
1384 {
1385 outtbuc[curouttbuc + kk] = (SCSUINT8_COP)outtbucptr[kk];
1386 }
1387 curouttbuc += sszz;
1388 break;
1389
1390 case SCSUINT16_N :
1391 outtbusptr = (SCSUINT16_COP *)outtbptr[ii]; /*uint16*/
1392 sszz = outtbsz[ii] * outtbsz[ii + nlnk];
1393 for (kk = 0; kk < sszz; kk++)
1394 {
1395 outtbus[curouttbus + kk] = (SCSUINT16_COP)outtbusptr[kk];
1396 }
1397 curouttbus += sszz;
1398 break;
1399
1400 case SCSUINT32_N :
1401 outtbulptr = (SCSUINT32_COP *)outtbptr[ii]; /*uint32*/
1402 sszz = outtbsz[ii] * outtbsz[ii + nlnk];
1403 for (kk = 0; kk < sszz; kk++)
1404 {
1405 outtbul[curouttbul + kk] = (SCSUINT32_COP)outtbulptr[kk];
1406 }
1407 curouttbul += sszz;
1408 break;
1409
1410 default : /* Add a message here */
1411 break;
1412 }
1413 }
1414 }
1415 *ierr = 20;
1416 freeouttbptr;
1417 } /* cosini_ */
1418
1419 /*--------------------------------------------------------------------------*/
cossim(double * told)1420 static void cossim(double *told)
1421 {
1422 /* System generated locals */
1423 int i3 = 0;
1424
1425 //** used for the [stop] button
1426 char* CommandToUnstack;
1427 static int CommandLength = 0;
1428 static int SeqSync = 0;
1429 static int one = 1;
1430
1431 /* Local variables */
1432 static scicos_flag flag__ = 0;
1433 static int ierr1 = 0;
1434 static int j = 0, k = 0;
1435 static double t = 0.;
1436 static int jj = 0;
1437 static double rhotmp = 0., tstop = 0.;
1438 static int kpo = 0, kev = 0;
1439 int Discrete_Jump = 0;
1440 int *jroot = NULL, *zcros = NULL;
1441 realtype reltol = 0., abstol = 0.;
1442 N_Vector y = NULL;
1443 void *ode_mem = NULL;
1444 int flag = 0, flagr = 0;
1445 int cnt = 0;
1446 /* Saving solver number */
1447 int solver = C2F(cmsolver).solver;
1448 /* Defining function pointers, for more readability */
1449 void(* ODEFree) (void**);
1450 int (* ODE) (void*, realtype, N_Vector, realtype*, int);
1451 int (* ODEReInit) (void*, realtype, N_Vector);
1452 int (* ODESetMaxStep) (void*, realtype);
1453 int (* ODESetStopTime) (void*, realtype);
1454 int (* ODEGetRootInfo) (void*, int*);
1455 int (* ODESStolerances) (void*, realtype, realtype);
1456 /* Generic flags for stop mode */
1457 int ODE_NORMAL = 1; /* ODE_NORMAL = CV_NORMAL = LS_NORMAL = 1 */
1458 int ODE_ONE_STEP = 2; /* ODE_ONE_STEP = CV_ONE_STEP = LS_ONE_STEP = 2 */
1459 switch (solver)
1460 {
1461 case LSodar_Dynamic:
1462 ODEFree = &LSodarFree;
1463 ODE = &LSodar;
1464 ODEReInit = &LSodarReInit;
1465 ODESetMaxStep = &LSodarSetMaxStep;
1466 ODESetStopTime = &LSodarSetStopTime;
1467 ODEGetRootInfo = &LSodarGetRootInfo;
1468 ODESStolerances = &LSodarSStolerances;
1469 break;
1470 case CVode_BDF_Newton:
1471 case CVode_BDF_Functional:
1472 case CVode_Adams_Newton:
1473 case CVode_Adams_Functional:
1474 case Dormand_Prince:
1475 case Runge_Kutta:
1476 case Implicit_Runge_Kutta:
1477 case Crank_Nicolson:
1478 ODEFree = &CVodeFree;
1479 ODE = &CVode;
1480 ODEReInit = &CVodeReInit;
1481 ODESetMaxStep = &CVodeSetMaxStep;
1482 ODESetStopTime = &CVodeSetStopTime;
1483 ODEGetRootInfo = &CVodeGetRootInfo;
1484 ODESStolerances = &CVodeSStolerances;
1485 break;
1486 default: // Unknown solver number
1487 *ierr = 1000;
1488 return;
1489 }
1490
1491 jroot = NULL;
1492 if (ng > 0)
1493 {
1494 if ((jroot = MALLOC(sizeof(int) * ng)) == NULL )
1495 {
1496 *ierr = 10000;
1497 return;
1498 }
1499 }
1500
1501 for ( jj = 0 ; jj < ng ; jj++ )
1502 {
1503 jroot[jj] = 0 ;
1504 }
1505
1506 zcros = NULL;
1507 if (ng > 0)
1508 {
1509 if ((zcros = MALLOC(sizeof(int) * ng)) == NULL )
1510 {
1511 *ierr = 10000;
1512 if (ng > 0)
1513 {
1514 FREE(jroot);
1515 }
1516 return;
1517 }
1518 }
1519
1520 reltol = (realtype) rtol;
1521 abstol = (realtype) Atol; /* Ith(abstol,1) = realtype) Atol;*/
1522
1523 if (*neq > 0) /* Unfortunately CVODE does not work with NEQ==0 */
1524 {
1525 y = N_VNewEmpty_Serial(*neq);
1526 if (check_flag((void *)y, "N_VNewEmpty_Serial", 0))
1527 {
1528 *ierr = 10000;
1529 if (ng > 0)
1530 {
1531 FREE(jroot);
1532 }
1533 if (ng > 0)
1534 {
1535 FREE(zcros);
1536 }
1537 return;
1538 }
1539
1540 NV_DATA_S(y) = x;
1541
1542 ode_mem = NULL;
1543
1544 /* Set extension of Sundials for scicos */
1545 set_sundials_with_extension(TRUE);
1546
1547 switch (solver)
1548 {
1549 case LSodar_Dynamic:
1550 ode_mem = LSodarCreate(neq, ng); /* Create the lsodar problem */
1551 break;
1552 case CVode_BDF_Newton:
1553 ode_mem = CVodeCreate(CV_BDF, CV_NEWTON);
1554 break;
1555 case CVode_BDF_Functional:
1556 ode_mem = CVodeCreate(CV_BDF, CV_FUNCTIONAL);
1557 break;
1558 case CVode_Adams_Newton:
1559 ode_mem = CVodeCreate(CV_ADAMS, CV_NEWTON);
1560 break;
1561 case CVode_Adams_Functional:
1562 ode_mem = CVodeCreate(CV_ADAMS, CV_FUNCTIONAL);
1563 break;
1564 case Dormand_Prince:
1565 ode_mem = CVodeCreate(CV_DOPRI, CV_FUNCTIONAL);
1566 break;
1567 case Runge_Kutta:
1568 ode_mem = CVodeCreate(CV_ExpRK, CV_FUNCTIONAL);
1569 break;
1570 case Implicit_Runge_Kutta:
1571 ode_mem = CVodeCreate(CV_ImpRK, CV_FUNCTIONAL);
1572 break;
1573 case Crank_Nicolson:
1574 ode_mem = CVodeCreate(CV_CRANI, CV_FUNCTIONAL);
1575 break;
1576 }
1577
1578 /* ode_mem = CVodeCreate(CV_ADAMS, CV_FUNCTIONAL);*/
1579
1580 if (check_flag((void *)ode_mem, "CVodeCreate", 0))
1581 {
1582 *ierr = 10000;
1583 N_VDestroy_Serial(y);
1584 FREE(jroot);
1585 FREE(zcros);
1586 return;
1587 }
1588
1589 if (solver == LSodar_Dynamic)
1590 {
1591 flag = LSodarSetErrHandlerFn(ode_mem, SundialsErrHandler, NULL);
1592 }
1593 else
1594 {
1595 flag = CVodeSetErrHandlerFn(ode_mem, SundialsErrHandler, NULL);
1596 }
1597 if (check_flag(&flag, "CVodeSetErrHandlerFn", 1))
1598 {
1599 *ierr = 300 + (-flag);
1600 freeall
1601 return;
1602 }
1603
1604 if (solver == LSodar_Dynamic)
1605 {
1606 flag = LSodarInit(ode_mem, simblklsodar, T0, y);
1607 }
1608 else
1609 {
1610 flag = CVodeInit (ode_mem, simblk, T0, y);
1611 }
1612 if (check_flag(&flag, "CVodeInit", 1))
1613 {
1614 *ierr = 300 + (-flag);
1615 freeall
1616 return;
1617 }
1618
1619 flag = ODESStolerances(ode_mem, reltol, abstol);
1620 if (check_flag(&flag, "CVodeSStolerances", 1))
1621 {
1622 *ierr = 300 + (-flag);
1623 freeall
1624 return;
1625 }
1626
1627 if (solver == LSodar_Dynamic)
1628 {
1629 flag = LSodarRootInit(ode_mem, ng, grblklsodar);
1630 }
1631 else
1632 {
1633 flag = CVodeRootInit(ode_mem, ng, grblk);
1634 }
1635 if (check_flag(&flag, "CVodeRootInit", 1))
1636 {
1637 *ierr = 300 + (-flag);
1638 freeall
1639 return;
1640 }
1641
1642 /* Call CVDense to specify the CVDENSE dense linear solver, only for solvers needing CVode's Newton method */
1643 if (solver == CVode_BDF_Newton || solver == CVode_Adams_Newton)
1644 {
1645 flag = CVDense(ode_mem, *neq);
1646 }
1647 if (check_flag(&flag, "CVDense", 1))
1648 {
1649 *ierr = 300 + (-flag);
1650 freeall
1651 return;
1652 }
1653
1654 if (hmax > 0)
1655 {
1656 flag = ODESetMaxStep(ode_mem, (realtype) hmax);
1657 if (check_flag(&flag, "CVodeSetMaxStep", 1))
1658 {
1659 *ierr = 300 + (-flag);
1660 freeall;
1661 return;
1662 }
1663 }
1664 /* Set the Jacobian routine to Jac (user-supplied)
1665 flag = CVDlsSetDenseJacFn(ode_mem, Jac);
1666 if (check_flag(&flag, "CVDlsSetDenseJacFn", 1)) return(1); */
1667
1668 }/* testing if neq>0 */
1669
1670 /* Function Body */
1671 C2F(coshlt).halt = 0;
1672 *ierr = 0;
1673
1674 /* initialization */
1675 C2F(realtimeinit)(told, &C2F(rtfactor).scale);
1676
1677 phase = 1;
1678 hot = 0;
1679
1680 jj = 0;
1681 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun)
1682 {
1683 if (Blocks[C2F(curblk).kfun - 1].ng >= 1)
1684 {
1685 zcros[jj] = C2F(curblk).kfun;
1686 ++jj;
1687 }
1688 }
1689 /* . ng >= jj required */
1690 if (jj != ng)
1691 {
1692 zcros[jj] = -1;
1693 }
1694 /* initialization (propagation of constant blocks outputs) */
1695 idoit(told);
1696 if (*ierr != 0)
1697 {
1698 freeall;
1699 return;
1700 }
1701 /*--discrete zero crossings----dzero--------------------*/
1702 if (ng > 0) /* storing ZC signs just after a solver call*/
1703 {
1704 /*zdoit(told, g, x, x);*/
1705 zdoit(told, x, x, g);
1706 if (*ierr != 0)
1707 {
1708 freeall;
1709 return;
1710 }
1711 for (jj = 0; jj < ng; ++jj)
1712 if (g[jj] >= 0)
1713 {
1714 jroot[jj] = 5;
1715 }
1716 else
1717 {
1718 jroot[jj] = -5;
1719 }
1720 }
1721 /*--discrete zero crossings----dzero--------------------*/
1722
1723 /* main loop on time */
1724 while (*told < *tf)
1725 {
1726 while (ismenu()) //** if the user has done something, do the actions
1727 {
1728 int ierr2 = 0;
1729 int iUnused;
1730 GetCommand(&CommandToUnstack, &SeqSync, &iUnused, NONE); //** get to the action
1731 CommandLength = (int)strlen(CommandToUnstack);
1732 //syncexec(CommandToUnstack, &CommandLength, &ierr2, &one, CommandLength); //** execute it
1733 FREE(CommandToUnstack);
1734 }
1735 if (C2F(coshlt).halt != 0)
1736 {
1737 if (C2F(coshlt).halt == 2)
1738 {
1739 *told = *tf; /* end simulation */
1740 }
1741 C2F(coshlt).halt = 0;
1742 freeall;
1743 return;
1744 }
1745 if (*pointi == 0)
1746 {
1747 t = *tf;
1748 }
1749 else
1750 {
1751 t = tevts[*pointi];
1752 }
1753 if (fabs(t - *told) < ttol)
1754 {
1755 t = *told;
1756 /* update output part */
1757 }
1758 if (*told > t)
1759 {
1760 /* ! scheduling problem */
1761 *ierr = 1;
1762 freeall;
1763 return;
1764 }
1765 if (*told != t)
1766 {
1767 if (xptr[nblk + 1] == 1)
1768 {
1769 /* . no continuous state */
1770 if (*told + deltat + ttol > t)
1771 {
1772 *told = t;
1773 }
1774 else
1775 {
1776 *told += deltat;
1777 }
1778 /* . update outputs of 'c' type blocks with no continuous state */
1779 if (*told >= *tf)
1780 {
1781 /* . we are at the end, update continuous part before leaving */
1782 if (ncord > 0)
1783 {
1784 cdoit(told);
1785 freeall;
1786 return;
1787 }
1788 }
1789 }
1790 else
1791 {
1792 /* integrate */
1793 rhotmp = *tf + ttol;
1794 if (*pointi != 0)
1795 {
1796 kpo = *pointi;
1797 L20:
1798 if (critev[kpo] == 1)
1799 {
1800 rhotmp = tevts[kpo];
1801 goto L30;
1802 }
1803 kpo = evtspt[kpo];
1804 if (kpo != 0)
1805 {
1806 goto L20;
1807 }
1808 L30:
1809 if (rhotmp < tstop)
1810 {
1811 hot = 0;
1812 }
1813 }
1814 tstop = rhotmp;
1815 t = Min(*told + deltat, Min(t, *tf + ttol));
1816
1817 if (ng > 0 && hot == 0 && nmod > 0)
1818 {
1819 zdoit(told, x, x, g);
1820 if (*ierr != 0)
1821 {
1822 freeall;
1823 return;
1824 }
1825 }
1826
1827 if (hot == 0) /* hot==0 : cold restart*/
1828 {
1829 flag = ODESetStopTime(ode_mem, (realtype)tstop); /* Setting the stop time*/
1830 if (check_flag(&flag, "CVodeSetStopTime", 1))
1831 {
1832 *ierr = 300 + (-flag);
1833 freeall;
1834 return;
1835 }
1836
1837 flag = ODEReInit(ode_mem, (realtype)(*told), y);
1838 if (check_flag(&flag, "CVodeReInit", 1))
1839 {
1840 *ierr = 300 + (-flag);
1841 freeall;
1842 return;
1843 }
1844 }
1845
1846 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
1847 {
1848 sciprint(_("****SUNDIALS.Cvode from: %f to %f hot= %d \n"), *told, t, hot);
1849 }
1850
1851 /*--discrete zero crossings----dzero--------------------*/
1852 /*--check for Dzeros after Mode settings or ddoit()----*/
1853 Discrete_Jump = 0;
1854
1855 if (ng > 0 && hot == 0)
1856 {
1857 zdoit(told, x, x, g);
1858 if (*ierr != 0)
1859 {
1860 freeall;
1861 return;
1862 }
1863 for (jj = 0; jj < ng; ++jj)
1864 {
1865 if ((g[jj] >= 0.0) && (jroot[jj] == -5))
1866 {
1867 Discrete_Jump = 1;
1868 jroot[jj] = 1;
1869 }
1870 else if ((g[jj] < 0.0) && (jroot[jj] == 5))
1871 {
1872 Discrete_Jump = 1;
1873 jroot[jj] = -1;
1874 }
1875 else
1876 {
1877 jroot[jj] = 0;
1878 }
1879 }
1880 }
1881 /*--discrete zero crossings----dzero--------------------*/
1882
1883 if (Discrete_Jump == 0) /* if there was a dzero, its event should be activated*/
1884 {
1885 phase = 2;
1886 flag = ODE(ode_mem, t, y, told, ODE_NORMAL);
1887 if (*ierr != 0)
1888 {
1889 freeall;
1890 return;
1891 }
1892 phase = 1;
1893 }
1894 else
1895 {
1896 flag = CV_ROOT_RETURN; /* in order to handle discrete jumps */
1897 }
1898
1899 /* . update outputs of 'c' type blocks if we are at the end*/
1900 if (*told >= *tf)
1901 {
1902 if (ncord > 0)
1903 {
1904 cdoit(told);
1905 freeall;
1906 return;
1907 }
1908 }
1909
1910 if (flag >= 0)
1911 {
1912 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
1913 {
1914 sciprint(_("****SUNDIALS.Cvode reached: %f\n"), *told);
1915 }
1916 hot = 1;
1917 cnt = 0;
1918 }
1919 else if ( flag == CV_TOO_MUCH_WORK || flag == CV_CONV_FAILURE || flag == CV_ERR_FAILURE)
1920 {
1921 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
1922 {
1923 sciprint(_("****SUNDIALS.Cvode: too much work at time=%g (stiff region, change RTOL and ATOL)\n"), *told);
1924 }
1925 hot = 0;
1926 cnt++;
1927 if (cnt > 5)
1928 {
1929 *ierr = 300 + (-flag);
1930 freeall;
1931 return;
1932 }
1933 }
1934 else
1935 {
1936 if (flag < 0)
1937 {
1938 *ierr = 300 + (-flag); /* raising errors due to internal errors, otherwise error due to flagr*/
1939 }
1940 freeall;
1941 return;
1942 }
1943
1944 if (flag == CV_ZERO_DETACH_RETURN)
1945 {
1946 hot = 0;
1947 }; /* new feature of sundials, detects zero-detaching */
1948
1949 if (flag == CV_ROOT_RETURN)
1950 {
1951 /* . at a least one root has been found */
1952 hot = 0;
1953 if (Discrete_Jump == 0)
1954 {
1955 flagr = ODEGetRootInfo(ode_mem, jroot);
1956 if (check_flag(&flagr, "CVodeGetRootInfo", 1))
1957 {
1958 *ierr = 300 + (-flagr);
1959 freeall;
1960 return;
1961 }
1962 }
1963 /* . at a least one root has been found */
1964 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
1965 {
1966 sciprint(_("root found at t=: %f\n"), *told);
1967 }
1968 /* . update outputs affecting ztyp blocks ONLY FOR OLD BLOCKS */
1969 zdoit(told, x, xd, g);
1970 if (*ierr != 0)
1971 {
1972 freeall;
1973 return;
1974 }
1975 for (jj = 0; jj < ng; ++jj)
1976 {
1977 C2F(curblk).kfun = zcros[ jj];
1978 if (C2F(curblk).kfun == -1)
1979 {
1980 break;
1981 }
1982 kev = 0;
1983
1984 for (j = zcptr[C2F(curblk).kfun] - 1 ;
1985 j < zcptr[C2F(curblk).kfun + 1] - 1 ; ++j)
1986 {
1987 if (jroot[j] != 0)
1988 {
1989 kev = 1;
1990 break;
1991 }
1992 }
1993 /* */
1994 if (kev != 0)
1995 {
1996 Blocks[C2F(curblk).kfun - 1].jroot = &jroot[zcptr[C2F(curblk).kfun] - 1];
1997 if (funtyp[C2F(curblk).kfun] > 0)
1998 {
1999
2000 if (Blocks[C2F(curblk).kfun - 1].nevout > 0)
2001 {
2002 flag__ = 3;
2003 if (Blocks[C2F(curblk).kfun - 1].nx > 0)
2004 {
2005 Blocks[C2F(curblk).kfun - 1].x = &x[xptr[C2F(curblk).kfun] - 1];
2006 Blocks[C2F(curblk).kfun - 1].xd = &xd[xptr[C2F(curblk).kfun] - 1];
2007 }
2008 /* call corresponding block to determine output event (kev) */
2009 Blocks[C2F(curblk).kfun - 1].nevprt = -kev;
2010 /*callf(told, xd, x, x,g,&flag__);*/
2011 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
2012 if (flag__ < 0)
2013 {
2014 *ierr = 5 - flag__;
2015 freeall;
2016 return;
2017 }
2018 /* . update event agenda */
2019 for (k = 0; k < Blocks[C2F(curblk).kfun - 1].nevout; ++k)
2020 {
2021 if (Blocks[C2F(curblk).kfun - 1].evout[k] >= 0.)
2022 {
2023 i3 = k + clkptr[C2F(curblk).kfun] ;
2024 addevs(Blocks[C2F(curblk).kfun - 1].evout[k] + (*told), &i3, &ierr1);
2025 if (ierr1 != 0)
2026 {
2027 /* . nevts too small */
2028 *ierr = 3;
2029 freeall;
2030 return;
2031 }
2032 }
2033 }
2034 }
2035 /* . update state */
2036 if (Blocks[C2F(curblk).kfun - 1].nx > 0)
2037 {
2038 /* . call corresponding block to update state */
2039 flag__ = 2;
2040 Blocks[C2F(curblk).kfun - 1].x = &x[xptr[C2F(curblk).kfun] - 1];
2041 Blocks[C2F(curblk).kfun - 1].xd = &xd[xptr[C2F(curblk).kfun] - 1];
2042 Blocks[C2F(curblk).kfun - 1].nevprt = -kev;
2043 /*callf(told, xd, x, x,g,&flag__);*/
2044 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
2045
2046 if (flag__ < 0)
2047 {
2048 *ierr = 5 - flag__;
2049 freeall;
2050 return;
2051 }
2052 }
2053 }
2054 }
2055 }
2056 }
2057 }
2058 /*--discrete zero crossings----dzero--------------------*/
2059 if (ng > 0) /* storing ZC signs just after a sundials call*/
2060 {
2061 zdoit(told, x, x, g);
2062 if (*ierr != 0)
2063 {
2064 freeall;
2065 return;
2066 }
2067 for (jj = 0; jj < ng; ++jj)
2068 {
2069 if (g[jj] >= 0)
2070 {
2071 jroot[jj] = 5;
2072 }
2073 else
2074 {
2075 jroot[jj] = -5;
2076 }
2077 }
2078 }
2079 /*--discrete zero crossings----dzero--------------------*/
2080
2081 C2F(realtime)(told);
2082 }
2083 else
2084 {
2085 /* . t==told */
2086 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
2087 {
2088 sciprint(_("Event: %d activated at t=%f\n"), *pointi, *told);
2089 for (kev = 0; kev < nblk; kev++)
2090 {
2091 if (Blocks[kev].nmode > 0)
2092 {
2093 sciprint(_("mode of block %d=%d, "), kev, Blocks[kev].mode[0]);
2094 }
2095 }
2096 sciprint(_("**mod**\n"));
2097 }
2098
2099 ddoit(told);
2100 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
2101 {
2102 sciprint(_("End of activation\n"));
2103 }
2104 if (*ierr != 0)
2105 {
2106 freeall;
2107 return;
2108 }
2109
2110 }
2111 /* end of main loop on time */
2112 }
2113 freeall;
2114 } /* cossim_ */
2115
2116 /*--------------------------------------------------------------------------*/
cossimdaskr(double * told)2117 static void cossimdaskr(double *told)
2118 {
2119 /* System generated locals */
2120 int i3;
2121 //** used for the [stop] button
2122 char* CommandToUnstack;
2123 static int CommandLength = 0;
2124 static int SeqSync = 0;
2125 static int one = 1;
2126
2127 /* Local variables */
2128 static scicos_flag flag__ = 0;
2129 static int ierr1 = 0;
2130 static int j = 0, k = 0;
2131 static double t = 0.;
2132 static int jj = 0;
2133 static double rhotmp = 0., tstop = 0.;
2134 static int kpo = 0, kev = 0;
2135
2136 int *jroot = NULL, *zcros = NULL;
2137 int *Mode_save = NULL;
2138 int Mode_change = 0;
2139
2140 int flag = 0, flagr = 0;
2141 N_Vector yy = NULL, yp = NULL;
2142 realtype reltol = 0., abstol = 0.;
2143 int Discrete_Jump = 0;
2144 N_Vector IDx = NULL;
2145 realtype *scicos_xproperty = NULL;
2146 DlsMat TJacque = NULL;
2147
2148 void *dae_mem = NULL;
2149 UserData data = NULL;
2150 IDAMem copy_IDA_mem = NULL;
2151 int maxnj = 0, maxnit = 0, maxnh = 0;
2152 /*-------------------- Analytical Jacobian memory allocation ----------*/
2153 int Jn = 0, Jnx = 0, Jno = 0, Jni = 0, Jactaille = 0;
2154 double uround = 0.;
2155 int cnt = 0, N_iters = 0;
2156 /* Saving solver number */
2157 int solver = C2F(cmsolver).solver;
2158 /* Flags for initial values calculation */
2159 int DAE_YA_YDP_INIT = 1;
2160 int DAE_Y_INIT = 2;
2161 /* Defining function pointers, for more readability*/
2162 void(* DAEFree) (void**);
2163 int (* DAESolve) (void*, realtype, realtype*, N_Vector, N_Vector, int);
2164 int (* DAEReInit) (void*, realtype, N_Vector, N_Vector);
2165 int (* DAESetId) (void*, N_Vector);
2166 int (* DAECalcIC) (void*, int, realtype);
2167 int (* DAESetMaxStep) (void*, realtype);
2168 int (* DAESetUserData) (void*, void*);
2169 int (* DAESetStopTime) (void*, realtype);
2170 int (* DAEGetRootInfo) (void*, int*);
2171 int (* DAESStolerances) (void*, realtype, realtype);
2172 int (* DAEGetConsistentIC) (void*, N_Vector, N_Vector);
2173 int (* DAESetMaxNumSteps) (void*, long int);
2174 int (* DAESetMaxNumJacsIC) (void*, int);
2175 int (* DAESetMaxNumItersIC) (void*, int);
2176 int (* DAESetMaxNumStepsIC) (void*, int);
2177 int (* DAESetLineSearchOffIC) (void*, int);
2178 /* For DAEs, the generic flags for stop mode depend on the used solver */
2179 int DAE_NORMAL = 0, DAE_ONE_STEP = 0;
2180 DAE_NORMAL = (solver == IDA_BDF_Newton) ? 1 : 0; /* IDA_NORMAL = 1, DDAS_NORMAL = 0 */
2181 DAE_ONE_STEP = (solver == IDA_BDF_Newton) ? 2 : 1; /* IDA_ONE_STEP = 2, DDAS_ONE_STEP = 1 */
2182 switch (solver)
2183 {
2184 case IDA_BDF_Newton:
2185 DAEFree = &IDAFree;
2186 DAESolve = &IDASolve;
2187 DAESetId = &IDASetId;
2188 DAEReInit = &IDAReInit;
2189 DAECalcIC = &IDACalcIC;
2190 DAESetMaxStep = &IDASetMaxStep;
2191 DAESetUserData = &IDASetUserData;
2192 DAESetStopTime = &IDASetStopTime;
2193 DAEGetRootInfo = &IDAGetRootInfo;
2194 DAESStolerances = &IDASStolerances;
2195 DAESetMaxNumSteps = &IDASetMaxNumSteps;
2196 DAEGetConsistentIC = &IDAGetConsistentIC;
2197 DAESetMaxNumJacsIC = &IDASetMaxNumJacsIC;
2198 DAESetMaxNumItersIC = &IDASetMaxNumItersIC;
2199 DAESetMaxNumStepsIC = &IDASetMaxNumStepsIC;
2200 DAESetLineSearchOffIC = &IDASetLineSearchOffIC;
2201 break;
2202 case DDaskr_BDF_Newton:
2203 case DDaskr_BDF_GMRes:
2204 DAEFree = &DDaskrFree;
2205 DAESolve = &DDaskrSolve;
2206 DAESetId = &DDaskrSetId;
2207 DAEReInit = &DDaskrReInit;
2208 DAECalcIC = &DDaskrCalcIC;
2209 DAESetMaxStep = &DDaskrSetMaxStep;
2210 DAESetUserData = &DDaskrSetUserData;
2211 DAESetStopTime = &DDaskrSetStopTime;
2212 DAEGetRootInfo = &DDaskrGetRootInfo;
2213 DAESStolerances = &DDaskrSStolerances;
2214 DAESetMaxNumSteps = &DDaskrSetMaxNumSteps;
2215 DAEGetConsistentIC = &DDaskrGetConsistentIC;
2216 DAESetMaxNumJacsIC = &DDaskrSetMaxNumJacsIC;
2217 DAESetMaxNumItersIC = &DDaskrSetMaxNumItersIC;
2218 DAESetMaxNumStepsIC = &DDaskrSetMaxNumStepsIC;
2219 DAESetLineSearchOffIC = &DDaskrSetLineSearchOffIC;
2220 break;
2221 default: // Unknown solver number
2222 *ierr = 1000;
2223 return;
2224 }
2225
2226 /* Set extension of Sundials for scicos */
2227 set_sundials_with_extension(TRUE);
2228
2229 // CI=1.0; /* for function Get_Jacobian_ci */
2230 jroot = NULL;
2231 if (ng > 0)
2232 {
2233 if ((jroot = MALLOC(sizeof(int) * ng)) == NULL )
2234 {
2235 *ierr = 10000;
2236 return;
2237 }
2238 }
2239 for ( jj = 0 ; jj < ng ; jj++ )
2240 {
2241 jroot[jj] = 0 ;
2242 }
2243
2244 zcros = NULL;
2245 if (ng > 0)
2246 {
2247 if ((zcros = MALLOC(sizeof(int) * ng)) == NULL )
2248 {
2249 *ierr = 10000;
2250 if (ng > 0)
2251 {
2252 FREE(jroot);
2253 }
2254 return;
2255 }
2256 }
2257
2258 Mode_save = NULL;
2259 if (nmod > 0)
2260 {
2261 if ((Mode_save = MALLOC(sizeof(int) * nmod)) == NULL )
2262 {
2263 *ierr = 10000;
2264 if (ng > 0)
2265 {
2266 FREE(jroot);
2267 }
2268 if (ng > 0)
2269 {
2270 FREE(zcros);
2271 }
2272 return;
2273 }
2274 }
2275
2276 reltol = (realtype) rtol;
2277 abstol = (realtype) Atol; /* Ith(abstol,1) = (realtype) Atol;*/
2278
2279 if (*neq > 0)
2280 {
2281 yy = NULL;
2282 yy = N_VNewEmpty_Serial(*neq);
2283 if (check_flag((void *)yy, "N_VNew_Serial", 0))
2284 {
2285 if (ng > 0)
2286 {
2287 FREE(jroot);
2288 }
2289 if (ng > 0)
2290 {
2291 FREE(zcros);
2292 }
2293 if (nmod > 0)
2294 {
2295 FREE(Mode_save);
2296 }
2297 }
2298 NV_DATA_S(yy) = x;
2299
2300 yp = NULL;
2301 yp = N_VNewEmpty_Serial(*neq);
2302 if (check_flag((void *)yp, "N_VNew_Serial", 0))
2303 {
2304 if (*neq > 0)
2305 {
2306 N_VDestroy_Serial(yy);
2307 }
2308 if (ng > 0)
2309 {
2310 FREE(jroot);
2311 }
2312 if (ng > 0)
2313 {
2314 FREE(zcros);
2315 }
2316 if (nmod > 0)
2317 {
2318 FREE(Mode_save);
2319 }
2320 return;
2321 }
2322 NV_DATA_S(yp) = xd;
2323
2324 IDx = NULL;
2325 IDx = N_VNew_Serial(*neq);
2326 if (check_flag((void *)IDx, "N_VNew_Serial", 0))
2327 {
2328 *ierr = 10000;
2329 if (*neq > 0)
2330 {
2331 N_VDestroy_Serial(yp);
2332 }
2333 if (*neq > 0)
2334 {
2335 N_VDestroy_Serial(yy);
2336 }
2337 if (ng > 0)
2338 {
2339 FREE(jroot);
2340 }
2341 if (ng > 0)
2342 {
2343 FREE(zcros);
2344 }
2345 if (nmod > 0)
2346 {
2347 FREE(Mode_save);
2348 }
2349 return;
2350 }
2351
2352 /* Call the Create and Init functions to initialize DAE memory */
2353 dae_mem = NULL;
2354 if (solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes)
2355 {
2356 dae_mem = DDaskrCreate(neq, ng, solver);
2357 }
2358 else
2359 {
2360 dae_mem = IDACreate();
2361 }
2362 if (check_flag((void *)dae_mem, "IDACreate", 0))
2363 {
2364 if (*neq > 0)
2365 {
2366 N_VDestroy_Serial(IDx);
2367 }
2368 if (*neq > 0)
2369 {
2370 N_VDestroy_Serial(yp);
2371 }
2372 if (*neq > 0)
2373 {
2374 N_VDestroy_Serial(yy);
2375 }
2376 if (ng > 0)
2377 {
2378 FREE(jroot);
2379 }
2380 if (ng > 0)
2381 {
2382 FREE(zcros);
2383 }
2384 if (nmod > 0)
2385 {
2386 FREE(Mode_save);
2387 }
2388 return;
2389 }
2390 if (C2F(cmsolver).solver == 100)
2391 {
2392 copy_IDA_mem = (IDAMem) dae_mem;
2393 }
2394
2395 if (solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes)
2396 {
2397 flag = DDaskrSetErrHandlerFn(dae_mem, SundialsErrHandler, NULL);
2398 }
2399 else
2400 {
2401 flag = IDASetErrHandlerFn(dae_mem, SundialsErrHandler, NULL);
2402 }
2403 if (check_flag(&flag, "IDASetErrHandlerFn", 1))
2404 {
2405 *ierr = 200 + (-flag);
2406 if (*neq > 0)
2407 {
2408 DAEFree(&dae_mem);
2409 }
2410 if (*neq > 0)
2411 {
2412 N_VDestroy_Serial(IDx);
2413 }
2414 if (*neq > 0)
2415 {
2416 N_VDestroy_Serial(yp);
2417 }
2418 if (*neq > 0)
2419 {
2420 N_VDestroy_Serial(yy);
2421 }
2422 if (ng > 0)
2423 {
2424 FREE(jroot);
2425 }
2426 if (ng > 0)
2427 {
2428 FREE(zcros);
2429 }
2430 if (nmod > 0)
2431 {
2432 FREE(Mode_save);
2433 }
2434 return;
2435 }
2436
2437 if (solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes)
2438 {
2439 flag = DDaskrInit(dae_mem, simblkddaskr, T0, yy, yp, jacpsol, psol);
2440 }
2441 else
2442 {
2443 flag = IDAInit(dae_mem, simblkdaskr, T0, yy, yp);
2444 }
2445 if (check_flag(&flag, "IDAInit", 1))
2446 {
2447 *ierr = 200 + (-flag);
2448 if (*neq > 0)
2449 {
2450 DAEFree(&dae_mem);
2451 }
2452 if (*neq > 0)
2453 {
2454 N_VDestroy_Serial(IDx);
2455 }
2456 if (*neq > 0)
2457 {
2458 N_VDestroy_Serial(yp);
2459 }
2460 if (*neq > 0)
2461 {
2462 N_VDestroy_Serial(yy);
2463 }
2464 if (ng > 0)
2465 {
2466 FREE(jroot);
2467 }
2468 if (ng > 0)
2469 {
2470 FREE(zcros);
2471 }
2472 if (nmod > 0)
2473 {
2474 FREE(Mode_save);
2475 }
2476 return;
2477 }
2478
2479 flag = DAESStolerances(dae_mem, reltol, abstol);
2480 if (check_flag(&flag, "IDASStolerances", 1))
2481 {
2482 *ierr = 200 + (-flag);
2483 if (*neq > 0)
2484 {
2485 DAEFree(&dae_mem);
2486 }
2487 if (*neq > 0)
2488 {
2489 N_VDestroy_Serial(IDx);
2490 }
2491 if (*neq > 0)
2492 {
2493 N_VDestroy_Serial(yp);
2494 }
2495 if (*neq > 0)
2496 {
2497 N_VDestroy_Serial(yy);
2498 }
2499 if (ng > 0)
2500 {
2501 FREE(jroot);
2502 }
2503 if (ng > 0)
2504 {
2505 FREE(zcros);
2506 }
2507 if (nmod > 0)
2508 {
2509 FREE(Mode_save);
2510 }
2511 return;
2512 }
2513
2514 if (solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes)
2515 {
2516 flag = DDaskrRootInit(dae_mem, ng, grblkddaskr);
2517 }
2518 else
2519 {
2520 flag = IDARootInit(dae_mem, ng, grblkdaskr);
2521 }
2522 if (check_flag(&flag, "IDARootInit", 1))
2523 {
2524 *ierr = 200 + (-flag);
2525 if (*neq > 0)
2526 {
2527 DAEFree(&dae_mem);
2528 }
2529 if (*neq > 0)
2530 {
2531 N_VDestroy_Serial(IDx);
2532 }
2533 if (*neq > 0)
2534 {
2535 N_VDestroy_Serial(yp);
2536 }
2537 if (*neq > 0)
2538 {
2539 N_VDestroy_Serial(yy);
2540 }
2541 if (ng > 0)
2542 {
2543 FREE(jroot);
2544 }
2545 if (ng > 0)
2546 {
2547 FREE(zcros);
2548 }
2549 if (nmod > 0)
2550 {
2551 FREE(Mode_save);
2552 }
2553 return;
2554 }
2555
2556 if (solver == IDA_BDF_Newton)
2557 {
2558 flag = IDADense(dae_mem, *neq);
2559 }
2560 if (check_flag(&flag, "IDADense", 1))
2561 {
2562 *ierr = 200 + (-flag);
2563 if (*neq > 0)if (solver == IDA_BDF_Newton)
2564 {
2565 IDAFree(&dae_mem);
2566 }
2567 if (*neq > 0)
2568 {
2569 N_VDestroy_Serial(IDx);
2570 }
2571 if (*neq > 0)
2572 {
2573 N_VDestroy_Serial(yp);
2574 }
2575 if (*neq > 0)
2576 {
2577 N_VDestroy_Serial(yy);
2578 }
2579 if (ng > 0)
2580 {
2581 FREE(jroot);
2582 }
2583 if (ng > 0)
2584 {
2585 FREE(zcros);
2586 }
2587 if (nmod > 0)
2588 {
2589 FREE(Mode_save);
2590 }
2591 return;
2592 }
2593
2594 data = NULL;
2595 if ((data = (UserData) MALLOC(sizeof(*data))) == NULL)
2596 {
2597 *ierr = 10000;
2598 if (*neq > 0)
2599 {
2600 IDAFree(&dae_mem);
2601 }
2602 if (*neq > 0)
2603 {
2604 N_VDestroy_Serial(IDx);
2605 }
2606 if (*neq > 0)
2607 {
2608 N_VDestroy_Serial(yp);
2609 }
2610 if (*neq > 0)
2611 {
2612 N_VDestroy_Serial(yy);
2613 }
2614 if (ng > 0)
2615 {
2616 FREE(jroot);
2617 }
2618 if (ng > 0)
2619 {
2620 FREE(zcros);
2621 }
2622 if (nmod > 0)
2623 {
2624 FREE(Mode_save);
2625 }
2626 return;
2627 }
2628 data->dae_mem = dae_mem;
2629 data->ewt = NULL;
2630 data->iwork = NULL;
2631 data->rwork = NULL;
2632 data->gwork = NULL;
2633
2634 data->ewt = N_VNew_Serial(*neq);
2635 if (check_flag((void *)data->ewt, "N_VNew_Serial", 0))
2636 {
2637 *ierr = 200 + (-flag);
2638 if (*neq > 0)
2639 {
2640 FREE(data);
2641 }
2642 if (*neq > 0)
2643 {
2644 DAEFree(&dae_mem);
2645 }
2646 if (*neq > 0)
2647 {
2648 N_VDestroy_Serial(IDx);
2649 }
2650 if (*neq > 0)
2651 {
2652 N_VDestroy_Serial(yp);
2653 }
2654 if (*neq > 0)
2655 {
2656 N_VDestroy_Serial(yy);
2657 }
2658 if (ng > 0)
2659 {
2660 FREE(jroot);
2661 }
2662 if (ng > 0)
2663 {
2664 FREE(zcros);
2665 }
2666 if (nmod > 0)
2667 {
2668 FREE(Mode_save);
2669 }
2670 return;
2671 }
2672 if ( ng > 0 )
2673 {
2674 if ((data->gwork = (double *) MALLOC(ng * sizeof(double))) == NULL)
2675 {
2676 if (*neq > 0)
2677 {
2678 N_VDestroy_Serial(data->ewt);
2679 }
2680 if (*neq > 0)
2681 {
2682 FREE(data);
2683 }
2684 if (*neq > 0)
2685 {
2686 DAEFree(&dae_mem);
2687 }
2688 if (*neq > 0)
2689 {
2690 N_VDestroy_Serial(IDx);
2691 }
2692 if (*neq > 0)
2693 {
2694 N_VDestroy_Serial(yp);
2695 }
2696 if (*neq > 0)
2697 {
2698 N_VDestroy_Serial(yy);
2699 }
2700 if (ng > 0)
2701 {
2702 FREE(jroot);
2703 }
2704 if (ng > 0)
2705 {
2706 FREE(zcros);
2707 }
2708 if (nmod > 0)
2709 {
2710 FREE(Mode_save);
2711 }
2712 return;
2713 }
2714 }
2715 /*Jacobian_Flag=0; */
2716 if (AJacobian_block > 0) /* set by the block with A-Jac in flag-4 using Set_Jacobian_flag(1); */
2717 {
2718 Jn = *neq;
2719 Jnx = Blocks[AJacobian_block - 1].nx;
2720 Jno = Blocks[AJacobian_block - 1].nout;
2721 Jni = Blocks[AJacobian_block - 1].nin;
2722 }
2723 else
2724 {
2725 Jn = *neq;
2726 Jnx = 0;
2727 Jno = 0;
2728 Jni = 0;
2729 }
2730 Jactaille = 3 * Jn + (Jn + Jni) * (Jn + Jno) + Jnx * (Jni + 2 * Jn + Jno) + (Jn - Jnx) * (2 * (Jn - Jnx) + Jno + Jni) + 2 * Jni * Jno;
2731
2732 if ((data->rwork = (double *) MALLOC(Jactaille * sizeof(double))) == NULL)
2733 {
2734 if ( ng > 0 )
2735 {
2736 FREE(data->gwork);
2737 }
2738 if (*neq > 0)
2739 {
2740 N_VDestroy_Serial(data->ewt);
2741 }
2742 if (*neq > 0)
2743 {
2744 FREE(data);
2745 }
2746 if (*neq > 0)
2747 {
2748 DAEFree(&dae_mem);
2749 }
2750 if (*neq > 0)
2751 {
2752 N_VDestroy_Serial(IDx);
2753 }
2754 if (*neq > 0)
2755 {
2756 N_VDestroy_Serial(yp);
2757 }
2758 if (*neq > 0)
2759 {
2760 N_VDestroy_Serial(yy);
2761 }
2762 if (ng > 0)
2763 {
2764 FREE(jroot);
2765 }
2766 if (ng > 0)
2767 {
2768 FREE(zcros);
2769 }
2770 if (nmod > 0)
2771 {
2772 FREE(Mode_save);
2773 }
2774 *ierr = 10000;
2775 return;
2776 }
2777
2778 if (solver == IDA_BDF_Newton)
2779 {
2780 flag = IDADlsSetDenseJacFn(dae_mem, Jacobians);
2781 }
2782 if (check_flag(&flag, "IDADlsSetDenseJacFn", 1))
2783 {
2784 *ierr = 200 + (-flag);
2785 freeallx
2786 return;
2787 }
2788
2789 TJacque = (DlsMat) NewDenseMat(*neq, *neq);
2790
2791 flag = DAESetUserData(dae_mem, data);
2792 if (check_flag(&flag, "IDASetUserData", 1))
2793 {
2794 *ierr = 200 + (-flag);
2795 freeallx
2796 return;
2797 }
2798
2799 if (hmax > 0)
2800 {
2801 flag = DAESetMaxStep(dae_mem, (realtype) hmax);
2802 if (check_flag(&flag, "IDASetMaxStep", 1))
2803 {
2804 *ierr = 200 + (-flag);
2805 freeallx
2806 return;
2807 }
2808 }
2809
2810 maxnj = 100; /* setting the maximum number of Jacobian evaluations during a Newton step */
2811 flag = DAESetMaxNumJacsIC(dae_mem, maxnj);
2812 if (check_flag(&flag, "IDASetMaxNumJacsIC", 1))
2813 {
2814 *ierr = 200 + (-flag);
2815 freeallx
2816 return;
2817 }
2818
2819 maxnit = 10; /* setting the maximum number of Newton iterations in any attempt to solve CIC */
2820 if (C2F(cmsolver).solver == 102)
2821 {
2822 maxnit = 15; /* By default, the Krylov max iterations should be 15 */
2823 }
2824 flag = DAESetMaxNumItersIC(dae_mem, maxnit);
2825 if (check_flag(&flag, "IDASetMaxNumItersIC", 1))
2826 {
2827 *ierr = 200 + (-flag);
2828 freeallx
2829 return;
2830 }
2831
2832 /* setting the maximum number of steps in an integration interval */
2833 maxnh = 2000;
2834 flag = DAESetMaxNumSteps(dae_mem, maxnh);
2835 if (check_flag(&flag, "IDASetMaxNumSteps", 1))
2836 {
2837 *ierr = 200 + (-flag);
2838 freeallx
2839 return;
2840 }
2841
2842 } /* testing if neq>0 */
2843
2844 uround = 1.0;
2845 do
2846 {
2847 uround = uround * 0.5;
2848 }
2849 while ( 1.0 + uround != 1.0);
2850 uround = uround * 2.0;
2851 SQuround = sqrt(uround);
2852 /* Function Body */
2853
2854 C2F(coshlt).halt = 0;
2855 *ierr = 0;
2856 /* hot = .false. */
2857 phase = 1;
2858 hot = 0;
2859
2860 /* initialization */
2861 C2F(realtimeinit)(told, &C2F(rtfactor).scale);
2862 /* ATOL and RTOL are scalars */
2863
2864 jj = 0;
2865 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun)
2866 {
2867 if (Blocks[C2F(curblk).kfun - 1].ng >= 1)
2868 {
2869 zcros[jj] = C2F(curblk).kfun;
2870 ++jj;
2871 }
2872 }
2873 /* . ng >= jj required */
2874 if (jj != ng)
2875 {
2876 zcros[jj] = -1;
2877 }
2878 /* initialization (propagation of constant blocks outputs) */
2879 idoit(told);
2880 if (*ierr != 0)
2881 {
2882 freeallx;
2883 return;
2884 }
2885
2886 /*--discrete zero crossings----dzero--------------------*/
2887 if (ng > 0) /* storing ZC signs just after a solver call*/
2888 {
2889 zdoit(told, x, x, g);
2890 if (*ierr != 0)
2891 {
2892 freeallx;
2893 return;
2894 }
2895 for (jj = 0; jj < ng; ++jj)
2896 if (g[jj] >= 0)
2897 {
2898 jroot[jj] = 5;
2899 }
2900 else
2901 {
2902 jroot[jj] = -5;
2903 }
2904 }
2905 /* main loop on time */
2906 while (*told < *tf)
2907 {
2908 while (ismenu()) //** if the user has done something, do the actions
2909 {
2910 int ierr2 = 0;
2911 int iUnused;
2912 GetCommand(&CommandToUnstack, &SeqSync, &iUnused, NONE); //** get to the action
2913 CommandLength = (int)strlen(CommandToUnstack);
2914 //syncexec(CommandToUnstack, &CommandLength, &ierr2, &one, CommandLength); //** execute it
2915 FREE(CommandToUnstack);
2916 }
2917 if (C2F(coshlt).halt != 0)
2918 {
2919 if (C2F(coshlt).halt == 2)
2920 {
2921 *told = *tf; /* end simulation */
2922 }
2923 C2F(coshlt).halt = 0;
2924 freeallx;
2925 return;
2926 }
2927 if (*pointi == 0)
2928 {
2929 t = *tf;
2930 }
2931 else
2932 {
2933 t = tevts[*pointi];
2934 }
2935 if (fabs(t - *told) < ttol)
2936 {
2937 t = *told;
2938 /* update output part */
2939 }
2940 if (*told > t)
2941 {
2942 /* ! scheduling problem */
2943 *ierr = 1;
2944 freeallx;
2945 return;
2946 }
2947 if (*told != t)
2948 {
2949 if (xptr[nblk + 1] == 1)
2950 {
2951 /* . no continuous state */
2952 if (*told + deltat + ttol > t)
2953 {
2954 *told = t;
2955 }
2956 else
2957 {
2958 *told += deltat;
2959 }
2960 /* . update outputs of 'c' type blocks with no continuous state */
2961 if (*told >= *tf)
2962 {
2963 /* . we are at the end, update continuous part before leaving */
2964 cdoit(told);
2965 freeallx;
2966 return;
2967 }
2968 }
2969 else
2970 {
2971 rhotmp = *tf + ttol;
2972 if (*pointi != 0)
2973 {
2974 kpo = *pointi;
2975 L20:
2976 if (critev[kpo] == 1)
2977 {
2978 rhotmp = tevts[kpo];
2979 goto L30;
2980 }
2981 kpo = evtspt[kpo];
2982 if (kpo != 0)
2983 {
2984 goto L20;
2985 }
2986 L30:
2987 if (rhotmp < tstop)
2988 {
2989 hot = 0;/* Cold-restart the solver if the new TSTOP isn't beyong the previous one*/
2990 }
2991 }
2992 tstop = rhotmp;
2993 t = Min(*told + deltat, Min(t, *tf + ttol));
2994
2995 if (hot == 0) /* CIC calculation when hot==0 */
2996 {
2997
2998 /* Setting the stop time*/
2999 flag = DAESetStopTime(dae_mem, (realtype)tstop);
3000 if (check_flag(&flag, "IDASetStopTime", 1))
3001 {
3002 *ierr = 200 + (-flag);
3003 freeallx;
3004 return;
3005 }
3006
3007 if (ng > 0 && nmod > 0)
3008 {
3009 phase = 1;
3010 zdoit(told, x, xd, g);
3011 if (*ierr != 0)
3012 {
3013 freeallx;
3014 return;
3015 }
3016 }
3017
3018 /*----------ID setting/checking------------*/
3019 N_VConst(ONE, IDx); /* Initialize id to 1's. */
3020 scicos_xproperty = NV_DATA_S(IDx);
3021 reinitdoit(told);
3022 if (*ierr > 0)
3023 {
3024 freeallx;
3025 return;
3026 }
3027 for (jj = 0; jj < *neq; jj++)
3028 {
3029 if (xprop[jj] == 1)
3030 {
3031 scicos_xproperty[jj] = ONE;
3032 }
3033 if (xprop[jj] == -1)
3034 {
3035 scicos_xproperty[jj] = ZERO;
3036 }
3037 }
3038 /* CI=0.0;CJ=100.0; // for functions Get_Jacobian_ci and Get_Jacobian_cj
3039 Jacobians(*neq, (realtype) (*told), yy, yp, bidon, (realtype) CJ, data, TJacque, tempv1, tempv2, tempv3);
3040 for (jj=0;jj<*neq;jj++){
3041 Jacque_col=DENSE_COL(TJacque,jj);
3042 CI=ZERO;
3043 for (kk=0;kk<*neq;kk++){
3044 if ((Jacque_col[kk]-Jacque_col[kk]!=0)) {
3045 CI=-ONE;
3046 break;
3047 }else{
3048 if (Jacque_col[kk]!=0){
3049 CI=ONE;
3050 break;
3051 }
3052 }
3053 }
3054 if (CI>=ZERO){ scicos_xproperty[jj]=CI;}else{fprintf(stderr,"\nWarinng! Xproperties are not match for i=%d!",jj);}
3055 } */
3056 /* printf("\n"); for(jj=0;jj<*neq;jj++) { printf("x%d=%g ",jj,scicos_xproperty[jj]); }*/
3057
3058 flag = DAESetId(dae_mem, IDx);
3059 if (check_flag(&flag, "IDASetId", 1))
3060 {
3061 *ierr = 200 + (-flag);
3062 freeallx
3063 return;
3064 }
3065 // CI=1.0; // for function Get_Jacobian_ci
3066 /*--------------------------------------------*/
3067 // maxnj=100; /* setting the maximum number of Jacobian evaluation during a Newton step */
3068 // flag=DAESetMaxNumJacsIC(dae_mem, maxnj);
3069 // if (check_flag(&flag, "IDASetMaxNumJacsIC", 1)) {
3070 // *ierr=200+(-flag);
3071 // freeallx;
3072 // return;
3073 // };
3074 // flag=DAESetLineSearchOffIC(dae_mem,FALSE); /* (def=false) */
3075 // if (check_flag(&flag, "IDASetLineSearchOffIC", 1)) {
3076 // *ierr=200+(-flag);
3077 // freeallx;
3078 // return;
3079 // };
3080 // flag=DAESetMaxNumItersIC(dae_mem, 10);/* (def=10) setting the maximum number of Newton iterations in any attempt to solve CIC */
3081 // if (check_flag(&flag, "IDASetMaxNumItersIC", 1)) {
3082 // *ierr=200+(-flag);
3083 // freeallx;
3084 // return;
3085 // };
3086
3087 N_iters = 4 + nmod * 4;
3088 for (j = 0; j <= N_iters; j++)
3089 {
3090 /* counter to reevaluate the
3091 modes in mode->CIC->mode->CIC-> loop
3092 do it once in the absence of mode (nmod=0) */
3093 /* updating the modes through Flag==9, Phase==1 */
3094
3095 /* Serge Steer 29/06/2009 */
3096 while (ismenu()) //** if the user has done something, do the actions
3097 {
3098 int ierr2 = 0;
3099 int iUnused;
3100 GetCommand(&CommandToUnstack, &SeqSync, &iUnused, NONE); //** get to the action
3101 CommandLength = (int)strlen(CommandToUnstack);
3102 //syncexec(CommandToUnstack, &CommandLength, &ierr2, &one, CommandLength); //** execute it
3103 FREE(CommandToUnstack);
3104 }
3105 if (C2F(coshlt).halt != 0)
3106 {
3107 if (C2F(coshlt).halt == 2)
3108 {
3109 *told = *tf; /* end simulation */
3110 }
3111 C2F(coshlt).halt = 0;
3112 freeallx;
3113 return;
3114 }
3115
3116 /* yy->PH */
3117 flag = DAEReInit(dae_mem, (realtype)(*told), yy, yp);
3118 if (check_flag(&flag, "IDAReInit", 1))
3119 {
3120 *ierr = 200 + (-flag);
3121 freeallx;
3122 return;
3123 }
3124
3125 phase = 2; /* IDACalcIC: PHI-> yy0: if (ok) yy0_cic-> PHI*/
3126 if (C2F(cmsolver).solver == 100)
3127 {
3128 copy_IDA_mem->ida_kk = 1;
3129 }
3130 // the initial conditons y0 and yp0 do not satisfy the DAE
3131 flagr = DAECalcIC(dae_mem, DAE_YA_YDP_INIT, (realtype)(t));
3132 phase = 1;
3133 flag = DAEGetConsistentIC(dae_mem, yy, yp); /* PHI->YY */
3134 if (*ierr > 5) /* *ierr>5 => singularity in block */
3135 {
3136 freeallx;
3137 return;
3138 }
3139
3140 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
3141 {
3142 if (flagr >= 0)
3143 {
3144 sciprint(_("**** SUNDIALS.IDA successfully initialized *****\n") );
3145 }
3146 else
3147 {
3148 sciprint(_("**** SUNDIALS.IDA failed to initialize ->try again *****\n") );
3149 }
3150 }
3151 /*-------------------------------------*/
3152 /* saving the previous modes*/
3153 for (jj = 0; jj < nmod; ++jj)
3154 {
3155 Mode_save[jj] = mod[jj];
3156 }
3157 if (ng > 0 && nmod > 0)
3158 {
3159 phase = 1;
3160 zdoit(told, x, xd, g);
3161 if (*ierr != 0)
3162 {
3163 freeallx;
3164 return;
3165 }
3166 }
3167 /*------------------------------------*/
3168 Mode_change = 0;
3169 for (jj = 0; jj < nmod; ++jj)
3170 {
3171 if (Mode_save[jj] != mod[jj])
3172 {
3173 Mode_change = 1;
3174 break;
3175 }
3176 }
3177 if (Mode_change == 0)
3178 {
3179 if (flagr >= 0 )
3180 {
3181 break; /* if (flagr>=0) break; else{ *ierr=200+(-flagr); freeallx; return; }*/
3182 }
3183 else if (j >= (int)( N_iters / 2))
3184 {
3185 /* DAESetMaxNumStepsIC(mem,10); */ /* maxnh (def=5) */
3186 DAESetMaxNumJacsIC(dae_mem, 10); /* maxnj 100 (def=4)*/
3187 /* DAESetMaxNumItersIC(mem,100000); */ /* maxnit in IDANewtonIC (def=10) */
3188 DAESetLineSearchOffIC(dae_mem, TRUE); /* (def=false) */
3189 /* DAESetNonlinConvCoefIC(mem,1.01);*/ /* (def=0.01-0.33*/
3190 flag = DAESetMaxNumItersIC(dae_mem, 1000);
3191 if (check_flag(&flag, "IDASetMaxNumItersIC", 1))
3192 {
3193 *ierr = 200 + (-flag);
3194 freeallx;
3195 return;
3196 };
3197 }
3198 }
3199 }/* mode-CIC counter*/
3200 if (Mode_change == 1)
3201 {
3202 /* In this case, we try again by relaxing all modes and calling DAECalcIC again
3203 /Masoud */
3204 phase = 1;
3205 if (C2F(cmsolver).solver == 100)
3206 {
3207 copy_IDA_mem->ida_kk = 1;
3208 }
3209 flagr = DAECalcIC(dae_mem, DAE_YA_YDP_INIT, (realtype)(t));
3210 phase = 1;
3211 flag = DAEGetConsistentIC(dae_mem, yy, yp); /* PHI->YY */
3212 if ((flagr < 0) || (*ierr > 5)) /* *ierr>5 => singularity in block */
3213 {
3214 *ierr = 23;
3215 freeallx;
3216 return;
3217 }
3218 }
3219 /*-----If flagr<0 the initialization solver has not converged-----*/
3220 if (flagr < 0)
3221 {
3222 *ierr = 237;
3223 freeallx;
3224 return;
3225 }
3226
3227 } /* CIC calculation when hot==0 */
3228
3229 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
3230 {
3231 sciprint(_("****daskr from: %f to %f hot= %d \n"), *told, t, hot);
3232 }
3233
3234 /*--discrete zero crossings----dzero--------------------*/
3235 /*--check for Dzeros after Mode settings or ddoit()----*/
3236 Discrete_Jump = 0;
3237 if (ng > 0 && hot == 0)
3238 {
3239 zdoit(told, x, xd, g);
3240 if (*ierr != 0)
3241 {
3242 freeallx;
3243 return;
3244 }
3245 for (jj = 0; jj < ng; ++jj)
3246 {
3247 if ((g[jj] >= 0.0) && ( jroot[jj] == -5))
3248 {
3249 Discrete_Jump = 1;
3250 jroot[jj] = 1;
3251 }
3252 else if ((g[jj] < 0.0) && ( jroot[jj] == 5))
3253 {
3254 Discrete_Jump = 1;
3255 jroot[jj] = -1;
3256 }
3257 else
3258 {
3259 jroot[jj] = 0;
3260 }
3261 }
3262 }
3263
3264 /*--discrete zero crossings----dzero--------------------*/
3265 if (Discrete_Jump == 0) /* if there was a dzero, its event should be activated*/
3266 {
3267 phase = 2;
3268 flagr = DAESolve(dae_mem, t, told, yy, yp, DAE_NORMAL);
3269 phase = 1;
3270 if (*ierr != 0)
3271 {
3272 freeallx;
3273 return;
3274 }
3275 }
3276 else
3277 {
3278 flagr = IDA_ROOT_RETURN; /* in order to handle discrete jumps */
3279 }
3280 if (flagr >= 0)
3281 {
3282 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
3283 {
3284 sciprint(_("****SUNDIALS.Ida reached: %f\n"), *told);
3285 }
3286 hot = 1;
3287 cnt = 0;
3288 }
3289 else if ( flagr == IDA_TOO_MUCH_WORK || flagr == IDA_CONV_FAIL || flagr == IDA_ERR_FAIL)
3290 {
3291 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
3292 {
3293 sciprint(_("**** SUNDIALS.Ida: too much work at time=%g (stiff region, change RTOL and ATOL)\n"), *told);
3294 }
3295 hot = 0;
3296 cnt++;
3297 if (cnt > 5)
3298 {
3299 *ierr = 200 + (-flagr);
3300 freeallx;
3301 return;
3302 }
3303 }
3304 else
3305 {
3306 if (flagr < 0)
3307 {
3308 *ierr = 200 + (-flagr); /* raising errors due to internal errors, otherwise error due to flagr*/
3309 }
3310 freeallx;
3311 return;
3312 }
3313
3314 /* update outputs of 'c' type blocks if we are at the end*/
3315 if (*told >= *tf)
3316 {
3317 cdoit(told);
3318 freeallx;
3319 return;
3320 }
3321
3322 if (flagr == IDA_ZERO_DETACH_RETURN)
3323 {
3324 hot = 0;
3325 }; /* new feature of sundials, detects unmasking */
3326 if (flagr == IDA_ROOT_RETURN)
3327 {
3328 /* . at least one root has been found */
3329 hot = 0;
3330 if (Discrete_Jump == 0)
3331 {
3332 flagr = DAEGetRootInfo(dae_mem, jroot);
3333 if (check_flag(&flagr, "IDAGetRootInfo", 1))
3334 {
3335 *ierr = 200 + (-flagr);
3336 freeallx;
3337 return;
3338 }
3339 }
3340
3341 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
3342 {
3343 sciprint(_("root found at t=: %f\n"), *told);
3344 }
3345 /* . update outputs affecting ztyp blocks ONLY FOR OLD BLOCKS*/
3346 zdoit(told, x, xd, g);
3347 if (*ierr != 0)
3348 {
3349 freeallx;
3350 return;
3351 }
3352 for (jj = 0; jj < ng; ++jj)
3353 {
3354 C2F(curblk).kfun = zcros[jj];
3355 if (C2F(curblk).kfun == -1)
3356 {
3357 break;
3358 }
3359 kev = 0;
3360 for (j = zcptr[C2F(curblk).kfun] - 1 ;
3361 j < zcptr[C2F(curblk).kfun + 1] - 1 ; ++j)
3362 {
3363 if (jroot[j] != 0)
3364 {
3365 kev = 1;
3366 break;
3367 }
3368 }
3369 if (kev != 0)
3370 {
3371 Blocks[C2F(curblk).kfun - 1].jroot = &jroot[zcptr[C2F(curblk).kfun] - 1];
3372 if (funtyp[C2F(curblk).kfun] > 0)
3373 {
3374 if (Blocks[C2F(curblk).kfun - 1].nevout > 0)
3375 {
3376 flag__ = 3;
3377 if (Blocks[C2F(curblk).kfun - 1].nx > 0)
3378 {
3379 Blocks[C2F(curblk).kfun - 1].x = &x[xptr[C2F(curblk).kfun] - 1];
3380 Blocks[C2F(curblk).kfun - 1].xd = &xd[xptr[C2F(curblk).kfun] - 1];
3381 }
3382 /* call corresponding block to determine output event (kev) */
3383 Blocks[C2F(curblk).kfun - 1].nevprt = -kev;
3384 /*callf(told, xd, x, x,g,&flag__);*/
3385 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
3386 if (flag__ < 0)
3387 {
3388 *ierr = 5 - flag__;
3389 freeallx;
3390 return;
3391 }
3392 /* update event agenda */
3393 for (k = 0; k < Blocks[C2F(curblk).kfun - 1].nevout; ++k)
3394 {
3395 if (Blocks[C2F(curblk).kfun - 1].evout[k] >= 0)
3396 {
3397 i3 = k + clkptr[C2F(curblk).kfun] ;
3398 addevs(Blocks[C2F(curblk).kfun - 1].evout[k] + (*told), &i3, &ierr1);
3399 if (ierr1 != 0)
3400 {
3401 /* . nevts too small */
3402 *ierr = 3;
3403 freeallx;
3404 return;
3405 }
3406 }
3407 }
3408 }
3409 /* update state */
3410 if ((Blocks[C2F(curblk).kfun - 1].nx > 0) || (*Blocks[C2F(curblk).kfun - 1].work != NULL) )
3411 {
3412 /* call corresponding block to update state */
3413 flag__ = 2;
3414 if (Blocks[C2F(curblk).kfun - 1].nx > 0)
3415 {
3416 Blocks[C2F(curblk).kfun - 1].x = &x[xptr[C2F(curblk).kfun] - 1];
3417 Blocks[C2F(curblk).kfun - 1].xd = &xd[xptr[C2F(curblk).kfun] - 1];
3418 }
3419 Blocks[C2F(curblk).kfun - 1].nevprt = -kev;
3420
3421 Blocks[C2F(curblk).kfun - 1].xprop = &xprop[-1 + xptr[C2F(curblk).kfun]];
3422 /*callf(told, xd, x, x,g,&flag__);*/
3423 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
3424
3425 if (flag__ < 0)
3426 {
3427 *ierr = 5 - flag__;
3428 freeallx;
3429 return;
3430 }
3431 for (j = 0; j < *neq; j++) /* Adjust xprop for IDx */
3432 {
3433 if (xprop[j] == 1)
3434 {
3435 scicos_xproperty[j] = ONE;
3436 }
3437 if (xprop[j] == -1)
3438 {
3439 scicos_xproperty[j] = ZERO;
3440 }
3441 }
3442 }
3443 }
3444 }
3445 }
3446 }
3447 /* Serge Steer 29/06/2009 */
3448 while (ismenu()) //** if the user has done something, do the actions
3449 {
3450 int ierr2 = 0;
3451 int iUnused;
3452 GetCommand(&CommandToUnstack, &SeqSync, &iUnused, NONE); //** get to the action
3453 CommandLength = (int)strlen(CommandToUnstack);
3454 //syncexec(CommandToUnstack, &CommandLength, &ierr2, &one, CommandLength); //** execute it
3455 FREE(CommandToUnstack);
3456 }
3457
3458 if (C2F(coshlt).halt != 0)
3459 {
3460 if (C2F(coshlt).halt == 2)
3461 {
3462 *told = *tf; /* end simulation */
3463 }
3464 C2F(coshlt).halt = 0;
3465 freeallx;
3466 return;
3467 }
3468 /* if(*pointi!=0){
3469 t=tevts[*pointi];
3470 if(*told<t-ttol){
3471 cdoit(told);
3472 goto L15;
3473 }
3474 }else{
3475 if(*told<*tf){
3476 cdoit(told);
3477 goto L15;
3478 }
3479 }*/
3480
3481 /*--discrete zero crossings----dzero--------------------*/
3482 if (ng > 0) /* storing ZC signs just after a ddaskr call*/
3483 {
3484 zdoit(told, x, xd, g);
3485 if (*ierr != 0)
3486 {
3487 freeallx;
3488 return;
3489 }
3490 for (jj = 0; jj < ng; ++jj)
3491 {
3492 if (g[jj] >= 0)
3493 {
3494 jroot[jj] = 5;
3495 }
3496 else
3497 {
3498 jroot[jj] = -5;
3499 }
3500 }
3501 }
3502 /*--discrete zero crossings----dzero--------------------*/
3503 }
3504 C2F(realtime)(told);
3505 }
3506 else
3507 {
3508 /* . t==told */
3509 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
3510 {
3511 sciprint(_("Event: %d activated at t=%f\n"), *pointi, *told);
3512 }
3513
3514 ddoit(told);
3515 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
3516 {
3517 sciprint(_("End of activation\n"));
3518 }
3519 if (*ierr != 0)
3520 {
3521 freeallx;
3522 return;
3523 }
3524 }
3525 /* end of main loop on time */
3526 }
3527 freeallx;
3528 } /* cossimdaskr_ */
3529 /*--------------------------------------------------------------------------*/
3530 /* Subroutine cosend */
cosend(double * told)3531 static void cosend(double *told)
3532 {
3533 /* Local variables */
3534 static scicos_flag flag__ = 0;
3535
3536 static int kfune = 0;
3537
3538 /* Function Body */
3539 *ierr = 0;
3540 /* loop on blocks */
3541 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun)
3542 {
3543 flag__ = 5;
3544 Blocks[C2F(curblk).kfun - 1].nevprt = 0;
3545 if (funtyp[C2F(curblk).kfun] >= 0)
3546 {
3547 if (Blocks[C2F(curblk).kfun - 1].nx > 0)
3548 {
3549 Blocks[C2F(curblk).kfun - 1].x = &x[xptr[C2F(curblk).kfun] - 1];
3550 Blocks[C2F(curblk).kfun - 1].xd = &xd[xptr[C2F(curblk).kfun] - 1];
3551 }
3552 /*callf(told, xd, x, x,x,&flag__);*/
3553 callf(told, &Blocks[C2F(curblk).kfun - 1], &flag__);
3554 if (flag__ < 0 && *ierr == 0)
3555 {
3556 *ierr = 5 - flag__;
3557 kfune = C2F(curblk).kfun;
3558 }
3559 }
3560 }
3561 if (*ierr != 0)
3562 {
3563 C2F(curblk).kfun = kfune;
3564 return;
3565 }
3566 } /* cosend_ */
3567 /*--------------------------------------------------------------------------*/
3568 /* callf */
callf(double * t,scicos_block * block,scicos_flag * flag)3569 void callf(double *t, scicos_block *block, scicos_flag *flag)
3570 {
3571 double* args[SZ_SIZE];
3572 int sz[SZ_SIZE];
3573 double intabl[TB_SIZE];
3574 double outabl[TB_SIZE];
3575
3576 int ii = 0, in = 0, out = 0, ki = 0, ko = 0, no = 0, ni = 0, k = 0, j = 0;
3577 int szi = 0, flagi = 0;
3578 double *ptr_d = NULL;
3579
3580 /* function pointers type def */
3581 voidf loc ;
3582 ScicosF0 loc0;
3583 ScicosF loc1;
3584 /* ScicosFm1 loc3;*/
3585 ScicosF2 loc2;
3586 ScicosF2z loc2z;
3587 ScicosFi loci1;
3588 ScicosFi2 loci2;
3589 ScicosFi2z loci2z;
3590 ScicosF4 loc4;
3591
3592 int solver = C2F(cmsolver).solver;
3593 int cosd = C2F(cosdebug).cosd;
3594 /*int kf = C2F(curblk).kfun;*/
3595 scicos_time = *t;
3596 block_error = (int*) flag;
3597
3598 /* debug block is never called */
3599 /*if (kf==(debug_block+1)) return;*/
3600 if (block->type == DEBUG_BLOCK)
3601 {
3602 return;
3603 }
3604
3605 /* flag 7 implicit initialization */
3606 flagi = (int) * flag;
3607 /* change flag to zero if flagi==7 for explicit block */
3608 if (flagi == 7 && block->type < 10000)
3609 {
3610 *flag = 0;
3611 }
3612
3613 /* display information for debugging mode */
3614 if (cosd > 1)
3615 {
3616 if (cosd != 3)
3617 {
3618 sciprint(_("block %d [%s] is called "), C2F(curblk).kfun, block->uid);
3619 sciprint(_("with flag %d "), *flag);
3620 sciprint(_("at time %f \n"), *t);
3621 }
3622 if (debug_block > -1)
3623 {
3624 if (cosd != 3)
3625 {
3626 sciprint(_("Entering the block \n"));
3627 }
3628 call_debug_scicos(block, flag, flagi, debug_block);
3629 if (*flag < 0)
3630 {
3631 return; /* error in debug block */
3632 }
3633 }
3634 }
3635
3636 C2F(scsptr).ptr = block->scsptr;
3637
3638 /* get pointer of the function */
3639 loc = block->funpt;
3640
3641 /* continuous state */
3642 if ((solver == IDA_BDF_Newton || solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes) && block->type < 10000 && *flag == 0)
3643 {
3644 ptr_d = block->xd;
3645 block->xd = block->res;
3646 }
3647
3648 /* switch loop */
3649 //sciprint("callf type=%d flag=%d\n",block->type,flagi);
3650 switch (block->type)
3651 {
3652 /*******************/
3653 /* function type 0 */
3654 /*******************/
3655 case 0 :
3656 {
3657 /* This is for compatibility */
3658 /* jroot is returned in g for old type */
3659 if (block->nevprt < 0)
3660 {
3661 for (j = 0; j < block->ng; ++j)
3662 {
3663 block->g[j] = (double)block->jroot[j];
3664 }
3665 }
3666
3667 /* concatenated entries and concatened outputs */
3668 /* catenate inputs if necessary */
3669 ni = 0;
3670 if (block->nin > 1)
3671 {
3672 ki = 0;
3673 for (in = 0; in < block->nin; in++)
3674 {
3675 szi = block->insz[in] * block->insz[in + block->nin];
3676 for (ii = 0; ii < szi; ii++)
3677 {
3678 intabl[ki++] = *((double *)(block->inptr[in]) + ii);
3679 }
3680 ni = ni + szi;
3681 }
3682 args[0] = &(intabl[0]);
3683 }
3684 else
3685 {
3686 if (block->nin == 0)
3687 {
3688 args[0] = NULL;
3689 }
3690 else
3691 {
3692 args[0] = (double *)(block->inptr[0]);
3693 ni = block->insz[0] * block->insz[1];
3694 }
3695 }
3696
3697 /* catenate outputs if necessary */
3698 no = 0;
3699 if (block->nout > 1)
3700 {
3701 ko = 0;
3702 for (out = 0; out < block->nout; out++)
3703 {
3704 szi = block->outsz[out] * block->outsz[out + block->nout];
3705 for (ii = 0; ii < szi; ii++)
3706 {
3707 outabl[ko++] = *((double *)(block->outptr[out]) + ii);
3708 }
3709 no = no + szi;
3710 }
3711 args[1] = &(outabl[0]);
3712 }
3713 else
3714 {
3715 if (block->nout == 0)
3716 {
3717 args[1] = NULL;
3718 }
3719 else
3720 {
3721 args[1] = (double *)(block->outptr[0]);
3722 no = block->outsz[0] * block->outsz[1];
3723 }
3724 }
3725
3726 loc0 = (ScicosF0) loc;
3727
3728 (*loc0)(flag, &block->nevprt, t, block->xd, block->x, &block->nx,
3729 block->z, &block->nz,
3730 block->evout, &block->nevout, block->rpar, &block->nrpar,
3731 block->ipar, &block->nipar, (double *)args[0], &ni,
3732 (double *)args[1], &no);
3733
3734 /* split output vector on each port if necessary */
3735 if (block->nout > 1)
3736 {
3737 ko = 0;
3738 for (out = 0; out < block->nout; out++)
3739 {
3740 szi = block->outsz[out] * block->outsz[out + block->nout];
3741 for (ii = 0; ii < szi; ii++)
3742 {
3743 *((double *)(block->outptr[out]) + ii) = outabl[ko++];
3744 }
3745 }
3746 }
3747
3748 /* adjust values of output register */
3749 for (in = 0; in < block->nevout; ++in)
3750 {
3751 block->evout[in] = block->evout[in] - *t;
3752 }
3753
3754 break;
3755 }
3756
3757 /*******************/
3758 /* function type 1 */
3759 /*******************/
3760 case 1 :
3761 {
3762 /* This is for compatibility */
3763 /* jroot is returned in g for old type */
3764 if (block->nevprt < 0)
3765 {
3766 for (j = 0; j < block->ng; ++j)
3767 {
3768 block->g[j] = (double)block->jroot[j];
3769 }
3770 }
3771
3772 /* one entry for each input or output */
3773 for (in = 0 ; in < block->nin ; in++)
3774 {
3775 args[in] = block->inptr[in];
3776 sz[in] = block->insz[in];
3777 }
3778 for (out = 0; out < block->nout; out++)
3779 {
3780 args[in + out] = block->outptr[out];
3781 sz[in + out] = block->outsz[out];
3782 }
3783 /* with zero crossing */
3784 if (block->ztyp > 0)
3785 {
3786 args[block->nin + block->nout] = block->g;
3787 sz[block->nin + block->nout] = block->ng;
3788 }
3789
3790 loc1 = (ScicosF) loc;
3791
3792 (*loc1)(flag, &block->nevprt, t, block->xd, block->x, &block->nx,
3793 block->z, &block->nz,
3794 block->evout, &block->nevout, block->rpar, &block->nrpar,
3795 block->ipar, &block->nipar,
3796 (double *)args[0], &sz[0],
3797 (double *)args[1], &sz[1], (double *)args[2], &sz[2],
3798 (double *)args[3], &sz[3], (double *)args[4], &sz[4],
3799 (double *)args[5], &sz[5], (double *)args[6], &sz[6],
3800 (double *)args[7], &sz[7], (double *)args[8], &sz[8],
3801 (double *)args[9], &sz[9], (double *)args[10], &sz[10],
3802 (double *)args[11], &sz[11], (double *)args[12], &sz[12],
3803 (double *)args[13], &sz[13], (double *)args[14], &sz[14],
3804 (double *)args[15], &sz[15], (double *)args[16], &sz[16],
3805 (double *)args[17], &sz[17]);
3806
3807 /* adjust values of output register */
3808 for (in = 0; in < block->nevout; ++in)
3809 {
3810 block->evout[in] = block->evout[in] - *t;
3811 }
3812
3813 break;
3814 }
3815
3816 /*******************/
3817 /* function type 2 */
3818 /*******************/
3819 case 2 :
3820 {
3821 /* This is for compatibility */
3822 /* jroot is returned in g for old type */
3823 if (block->nevprt < 0)
3824 {
3825 for (j = 0; j < block->ng; ++j)
3826 {
3827 block->g[j] = (double)block->jroot[j];
3828 }
3829 }
3830
3831 /* no zero crossing */
3832 if (block->ztyp == 0)
3833 {
3834 loc2 = (ScicosF2) loc;
3835 (*loc2)(flag, &block->nevprt, t, block->xd, block->x, &block->nx,
3836 block->z, &block->nz,
3837 block->evout, &block->nevout, block->rpar, &block->nrpar,
3838 block->ipar, &block->nipar, (double **)block->inptr,
3839 block->insz, &block->nin,
3840 (double **)block->outptr, block->outsz, &block->nout,
3841 block->scsptr);
3842 }
3843 /* with zero crossing */
3844 else
3845 {
3846 loc2z = (ScicosF2z) loc;
3847 (*loc2z)(flag, &block->nevprt, t, block->xd, block->x, &block->nx,
3848 block->z, &block->nz,
3849 block->evout, &block->nevout, block->rpar, &block->nrpar,
3850 block->ipar, &block->nipar, (double **)block->inptr,
3851 block->insz, &block->nin,
3852 (double **)block->outptr, block->outsz, &block->nout,
3853 block->scsptr,
3854 block->g, &block->ng);
3855 }
3856
3857 /* adjust values of output register */
3858 for (in = 0; in < block->nevout; ++in)
3859 {
3860 block->evout[in] = block->evout[in] - *t;
3861 }
3862
3863 break;
3864 }
3865
3866 /*******************/
3867 /* function type 4 */
3868 /*******************/
3869 case 4 :
3870 {
3871 /* get pointer of the function type 4*/
3872 loc4 = (ScicosF4) loc;
3873
3874 (*loc4)(block, *flag);
3875
3876 break;
3877 }
3878
3879 /***********************/
3880 /* function type 10001 */
3881 /***********************/
3882 case 10001 :
3883 {
3884 /* This is for compatibility */
3885 /* jroot is returned in g for old type */
3886 if (block->nevprt < 0)
3887 {
3888 for (j = 0; j < block->ng; ++j)
3889 {
3890 block->g[j] = (double)block->jroot[j];
3891 }
3892 }
3893
3894 /* implicit block one entry for each input or output */
3895 for (in = 0 ; in < block->nin ; in++)
3896 {
3897 args[in] = block->inptr[in];
3898 sz[in] = block->insz[in];
3899 }
3900 for (out = 0; out < block->nout; out++)
3901 {
3902 args[in + out] = block->outptr[out];
3903 sz[in + out] = block->outsz[out];
3904 }
3905 /* with zero crossing */
3906 if (block->ztyp > 0)
3907 {
3908 args[block->nin + block->nout] = block->g;
3909 sz[block->nin + block->nout] = block->ng;
3910 }
3911
3912 loci1 = (ScicosFi) loc;
3913 (*loci1)(flag, &block->nevprt, t, block->res, block->xd, block->x,
3914 &block->nx, block->z, &block->nz,
3915 block->evout, &block->nevout, block->rpar, &block->nrpar,
3916 block->ipar, &block->nipar,
3917 (double *)args[0], &sz[0],
3918 (double *)args[1], &sz[1], (double *)args[2], &sz[2],
3919 (double *)args[3], &sz[3], (double *)args[4], &sz[4],
3920 (double *)args[5], &sz[5], (double *)args[6], &sz[6],
3921 (double *)args[7], &sz[7], (double *)args[8], &sz[8],
3922 (double *)args[9], &sz[9], (double *)args[10], &sz[10],
3923 (double *)args[11], &sz[11], (double *)args[12], &sz[12],
3924 (double *)args[13], &sz[13], (double *)args[14], &sz[14],
3925 (double *)args[15], &sz[15], (double *)args[16], &sz[16],
3926 (double *)args[17], &sz[17]);
3927
3928 /* adjust values of output register */
3929 for (in = 0; in < block->nevout; ++in)
3930 {
3931 block->evout[in] = block->evout[in] - *t;
3932 }
3933
3934 break;
3935 }
3936
3937 /***********************/
3938 /* function type 10002 */
3939 /***********************/
3940 case 10002 :
3941 {
3942 /* This is for compatibility */
3943 /* jroot is returned in g for old type */
3944 if (block->nevprt < 0)
3945 {
3946 for (j = 0; j < block->ng; ++j)
3947 {
3948 block->g[j] = (double)block->jroot[j];
3949 }
3950 }
3951
3952 /* implicit block, inputs and outputs given by a table of pointers */
3953 /* no zero crossing */
3954 if (block->ztyp == 0)
3955 {
3956 loci2 = (ScicosFi2) loc;
3957 (*loci2)(flag, &block->nevprt, t, block->res,
3958 block->xd, block->x, &block->nx,
3959 block->z, &block->nz,
3960 block->evout, &block->nevout, block->rpar, &block->nrpar,
3961 block->ipar, &block->nipar, (double **)block->inptr,
3962 block->insz, &block->nin,
3963 (double **)block->outptr, block->outsz, &block->nout);
3964 }
3965 /* with zero crossing */
3966 else
3967 {
3968 loci2z = (ScicosFi2z) loc;
3969 (*loci2z)(flag, &block->nevprt, t, block->res,
3970 block->xd, block->x, &block->nx,
3971 block->z, &block->nz,
3972 block->evout, &block->nevout, block->rpar, &block->nrpar,
3973 block->ipar, &block->nipar,
3974 (double **)block->inptr, block->insz, &block->nin,
3975 (double **)block->outptr, block->outsz, &block->nout,
3976 block->g, &block->ng);
3977 }
3978
3979 /* adjust values of output register */
3980 for (in = 0; in < block->nevout; ++in)
3981 {
3982 block->evout[in] = block->evout[in] - *t;
3983 }
3984
3985 break;
3986 }
3987
3988 /***********************/
3989 /* function type 10004 */
3990 /***********************/
3991 case 10004 :
3992 {
3993 /* get pointer of the function type 4*/
3994 loc4 = (ScicosF4) loc;
3995
3996 (*loc4)(block, *flag);
3997
3998 break;
3999 }
4000
4001 /***********/
4002 /* default */
4003 /***********/
4004 default :
4005 {
4006 sciprint(_("Undefined Function type\n"));
4007 *flag = -1000;
4008 return; /* exit */
4009 }
4010 }
4011 // sciprint("callf end flag=%d\n",*flag);
4012 /* Implicit Solver & explicit block & flag==0 */
4013 /* adjust continuous state vector after call */
4014 if ((solver == IDA_BDF_Newton || solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes) && block->type < 10000 && *flag == 0)
4015 {
4016 block->xd = ptr_d;
4017 if (flagi != 7)
4018 {
4019 for (k = 0; k < block->nx; k++)
4020 {
4021 block->res[k] = block->res[k] - block->xd[k];
4022 }
4023 }
4024 else
4025 {
4026 for (k = 0; k < block->nx; k++)
4027 {
4028 block->xd[k] = block->res[k];
4029 }
4030 }
4031 }
4032
4033 /* debug block */
4034 if (cosd > 1)
4035 {
4036 if (debug_block > -1)
4037 {
4038 if (*flag < 0)
4039 {
4040 return; /* error in block */
4041 }
4042 if (cosd != 3)
4043 {
4044 sciprint(_("Leaving block %d \n"), C2F(curblk).kfun);
4045 }
4046 call_debug_scicos(block, flag, flagi, debug_block);
4047 /*call_debug_scicos(flag,kf,flagi,debug_block);*/
4048 }
4049 }
4050
4051 block_error = NULL;
4052 } /* callf */
4053 /*--------------------------------------------------------------------------*/
4054 /* call_debug_scicos */
call_debug_scicos(scicos_block * block,scicos_flag * flag,int flagi,int deb_blk)4055 static void call_debug_scicos(scicos_block *block, scicos_flag *flag, int flagi, int deb_blk)
4056 {
4057 voidf loc ;
4058 int solver = C2F(cmsolver).solver, k = 0;
4059 ScicosF4 loc4;
4060 double *ptr_d = NULL;
4061
4062 C2F(cosdebugcounter).counter = C2F(cosdebugcounter).counter + 1;
4063 C2F(scsptr).ptr = block->scsptr;
4064
4065 loc = Blocks[deb_blk].funpt; /* GLOBAL */
4066 loc4 = (ScicosF4) loc;
4067
4068 /* continuous state */
4069 if ((solver == IDA_BDF_Newton || solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes) && block->type < 10000 && *flag == 0)
4070 {
4071 ptr_d = block->xd;
4072 block->xd = block->res;
4073 }
4074
4075 // Temporarily replacing the block's computational function with DEBUG_BLOCK's so that sciblk4 will call %debug_scicos()
4076 block->scsptr = Blocks[deb_blk].scsptr;
4077 (*loc4)(block, *flag);
4078 block->scsptr = C2F(scsptr).ptr;
4079
4080 /* Implicit Solver & explicit block & flag==0 */
4081 /* adjust continuous state vector after call */
4082 if ((solver == IDA_BDF_Newton || solver == DDaskr_BDF_Newton || solver == DDaskr_BDF_GMRes) && block->type < 10000 && *flag == 0)
4083 {
4084 block->xd = ptr_d;
4085 if (flagi != 7)
4086 {
4087 for (k = 0; k < block->nx; k++)
4088 {
4089 block->res[k] = block->res[k] - block->xd[k];
4090 }
4091 }
4092 else
4093 {
4094 for (k = 0; k < block->nx; k++)
4095 {
4096 block->xd[k] = block->res[k];
4097 }
4098 }
4099 }
4100
4101 if (*flag < 0)
4102 {
4103 sciprint(_("Error in the Debug block \n"));
4104 }
4105 } /* call_debug_scicos */
4106 /*--------------------------------------------------------------------------*/
4107 /* simblk */
simblk(realtype t,N_Vector yy,N_Vector yp,void * f_data)4108 static int simblk(realtype t, N_Vector yy, N_Vector yp, void *f_data)
4109 {
4110 double tx = 0., *x = NULL, *xd = NULL;
4111 int i = 0, nantest = 0;
4112
4113 tx = (double) t;
4114 x = (double *) NV_DATA_S(yy);
4115 xd = (double *) NV_DATA_S(yp);
4116
4117 for (i = 0; i < *neq; i++)
4118 {
4119 xd[i] = 0; /* à la place de "C2F(dset)(neq, &c_b14,xcdot , &c__1);"*/
4120 }
4121 C2F(ierode).iero = 0;
4122 *ierr = 0;
4123 odoit(&tx, x, xd, xd);
4124 C2F(ierode).iero = *ierr;
4125
4126 if (*ierr == 0)
4127 {
4128 nantest = 0;
4129 for (i = 0; i < *neq; i++) /* NaN checking */
4130 {
4131 if ((xd[i] - xd[i] != 0))
4132 {
4133 Sciwarning(_("\nWarning: The computing function #%d returns a NaN/Inf"), i);
4134 nantest = 1;
4135 break;
4136 }
4137 }
4138 if (nantest == 1)
4139 {
4140 return 349; /* recoverable error; */
4141 }
4142 }
4143
4144 return (abs(*ierr)); /* ierr>0 recoverable error; ierr>0 unrecoverable error; ierr=0: ok*/
4145
4146 } /* simblk */
4147 /*--------------------------------------------------------------------------*/
4148 /* grblk */
grblk(realtype t,N_Vector yy,realtype * gout,void * g_data)4149 static int grblk(realtype t, N_Vector yy, realtype *gout, void *g_data)
4150 {
4151 double tx = 0., *x = NULL;
4152 int jj = 0, nantest = 0;
4153
4154 tx = (double) t;
4155 x = (double *) NV_DATA_S(yy);
4156
4157 C2F(ierode).iero = 0;
4158 *ierr = 0;
4159
4160 zdoit(&tx, x, x, (double*) gout);
4161
4162 if (*ierr == 0)
4163 {
4164 nantest = 0;
4165 for (jj = 0; jj < ng; jj++)
4166 if (gout[jj] - gout[jj] != 0)
4167 {
4168 Sciwarning(_("\nWarning: The zero_crossing function #%d returns a NaN/Inf"), jj);
4169 nantest = 1;
4170 break;
4171 } /* NaN checking */
4172 if (nantest == 1)
4173 {
4174 return 350; /* recoverable error; */
4175 }
4176 }
4177 C2F(ierode).iero = *ierr;
4178
4179 return 0;
4180 } /* grblk */
4181 /*--------------------------------------------------------------------------*/
4182 /* simblklsodar */
simblklsodar(int * nequations,realtype * tOld,realtype * actual,realtype * res)4183 static void simblklsodar(int * nequations, realtype * tOld, realtype * actual, realtype * res)
4184 {
4185 double tx = 0.;
4186 int i = 0;
4187
4188 tx = (double) * tOld;
4189
4190 for (i = 0; i < *nequations; ++i)
4191 {
4192 res[i] = 0; /* à la place de "C2F(dset)(neq, &c_b14,xcdot , &c__1);"*/
4193 }
4194 C2F(ierode).iero = 0;
4195 *ierr = 0;
4196 odoit(&tx, actual, res, res);
4197 C2F(ierode).iero = *ierr;
4198
4199 if (*ierr == 0)
4200 {
4201 for (i = 0; i < *nequations; i++) /* NaN checking */
4202 {
4203 if ((res[i] - res[i] != 0))
4204 {
4205 Sciwarning(_("\nWarning: The computing function #%d returns a NaN/Inf"), i);
4206 }
4207 }
4208 }
4209 } /* simblklsodar */
4210 /*--------------------------------------------------------------------------*/
4211 /* grblklsodar */
grblklsodar(int * nequations,realtype * tOld,realtype * actual,int * ngc,realtype * res)4212 static void grblklsodar(int * nequations, realtype * tOld, realtype * actual, int * ngc, realtype * res)
4213 {
4214 double tx = 0.;
4215 int jj = 0;
4216
4217 tx = (double) * tOld;
4218
4219 C2F(ierode).iero = 0;
4220 *ierr = 0;
4221
4222 zdoit(&tx, actual, actual, res);
4223
4224 if (*ierr == 0)
4225 {
4226 for (jj = 0; jj < *ngc; jj++)
4227 {
4228 if (res[jj] - res[jj] != 0)
4229 {
4230 Sciwarning(_("\nWarning: The zero_crossing function #%d returns a NaN/Inf"), jj);
4231 } /* NaN checking */
4232 }
4233 }
4234 } /* grblklsodar */
4235 /*--------------------------------------------------------------------------*/
4236 /* simblkdaskr */
simblkdaskr(realtype tres,N_Vector yy,N_Vector yp,N_Vector resval,void * rdata)4237 static int simblkdaskr(realtype tres, N_Vector yy, N_Vector yp, N_Vector resval, void *rdata)
4238 {
4239 double tx = 0.;
4240 double *xc = NULL, *xcdot = NULL, *residual = NULL;
4241 realtype alpha = 0.;
4242
4243 UserData data;
4244
4245 realtype hh = 0.;
4246 int qlast = 0;
4247 int jj = 0, flag = 0, nantest = 0;
4248
4249 data = (UserData) rdata;
4250
4251 if (get_phase_simulation() == 1)
4252 {
4253 /* Just to update mode in a very special case, i.e., when initialization using modes fails.
4254 in this case, we relax all modes and try again one more time.
4255 */
4256 zdoit(&tx, NV_DATA_S(yy), NV_DATA_S(yp), NULL);
4257 }
4258
4259 hh = ZERO;
4260 flag = IDAGetCurrentStep(data->dae_mem, &hh);
4261 if (flag < 0)
4262 {
4263 *ierr = 200 + (-flag);
4264 return (*ierr);
4265 };
4266
4267 qlast = 0;
4268 flag = IDAGetCurrentOrder(data->dae_mem, &qlast);
4269 if (flag < 0)
4270 {
4271 *ierr = 200 + (-flag);
4272 return (*ierr);
4273 };
4274
4275 alpha = ZERO;
4276 for (jj = 0; jj < qlast; jj++)
4277 {
4278 alpha = alpha - ONE / (jj + 1);
4279 }
4280 if (hh != 0)
4281 // CJ=-alpha/hh; // For function Get_Jacobian_cj
4282 {
4283 CJJ = -alpha / hh;
4284 }
4285 else
4286 {
4287 *ierr = 217;
4288 return (*ierr);
4289 }
4290 xc = (double *) NV_DATA_S(yy);
4291 xcdot = (double *) NV_DATA_S(yp);
4292 residual = (double *) NV_DATA_S(resval);
4293 tx = (double) tres;
4294
4295 C2F(dcopy)(neq, xcdot, &c__1, residual, &c__1);
4296 *ierr = 0;
4297 C2F(ierode).iero = 0;
4298 odoit(&tx, xc, xcdot, residual);
4299
4300 C2F(ierode).iero = *ierr;
4301
4302 if (*ierr == 0)
4303 {
4304 nantest = 0;
4305 for (jj = 0; jj < *neq; jj++)
4306 if (residual[jj] - residual[jj] != 0) /* NaN checking */
4307 {
4308 //sciprint(_("\nWarning: The residual function #%d returns a NaN"),jj);
4309 nantest = 1;
4310 break;
4311 }
4312 if (nantest == 1)
4313 {
4314 return 257; /* recoverable error; */
4315 }
4316 }
4317
4318 return (abs(*ierr)); /* ierr>0 recoverable error; ierr>0 unrecoverable error; ierr=0: ok*/
4319 }/* simblkdaskr */
4320 /*--------------------------------------------------------------------------*/
4321 /* grblkdaskr */
grblkdaskr(realtype t,N_Vector yy,N_Vector yp,realtype * gout,void * g_data)4322 static int grblkdaskr(realtype t, N_Vector yy, N_Vector yp, realtype *gout, void *g_data)
4323 {
4324 double tx = 0.;
4325 int jj = 0, nantest = 0;
4326
4327 tx = (double) t;
4328
4329 *ierr = 0;
4330 C2F(ierode).iero = 0;
4331 zdoit(&tx, NV_DATA_S(yy), NV_DATA_S(yp), (double *) gout);
4332 if (*ierr == 0)
4333 {
4334 nantest = 0; /* NaN checking */
4335 for (jj = 0; jj < ng; jj++)
4336 {
4337 if (gout[jj] - gout[jj] != 0)
4338 {
4339 Sciwarning(_("\nWarning: The zero-crossing function #%d returns a NaN"), jj);
4340 nantest = 1;
4341 break;
4342 }
4343 }
4344 if (nantest == 1)
4345 {
4346 return 258; /* recoverable error; */
4347 }
4348 }
4349 C2F(ierode).iero = *ierr;
4350 return (*ierr);
4351 }/* grblkdaskr */
4352 /*--------------------------------------------------------------------------*/
4353 /* simblkddaskr */
simblkddaskr(realtype * tOld,realtype * actual,realtype * actualP,realtype * res,int * flag,double * dummy1,int * dummy2)4354 static void simblkddaskr(realtype *tOld, realtype *actual, realtype *actualP, realtype *res, int *flag, double *dummy1, int *dummy2)
4355 {
4356 double tx = 0.;
4357
4358 int jj = 0;
4359
4360 if (get_phase_simulation() == 1)
4361 {
4362 /* Just to update mode in a very special case, i.e., when initialization using modes fails.
4363 in this case, we relax all modes and try again one more time.
4364 */
4365 zdoit(&tx, actual, actualP, NULL);
4366 }
4367
4368 CJJ = 6;
4369
4370 tx = (double) * tOld;
4371 *flag = 0;
4372
4373 C2F(dcopy)(neq, actualP, &c__1, res, &c__1);
4374 *ierr = 0;
4375 C2F(ierode).iero = 0;
4376 odoit(&tx, actual, actualP, res);
4377 C2F(ierode).iero = *ierr;
4378
4379 if (*ierr == 0)
4380 {
4381 for (jj = 0; jj < *neq; jj++)
4382 if (res[jj] - res[jj] != 0) /* NaN checking */
4383 {
4384 Sciwarning(_("\nWarning: The residual function #%d returns a NaN"), jj);
4385 *flag = -1; /* recoverable error; */
4386 return;
4387 }
4388 }
4389 else
4390 {
4391 *flag = -2;
4392 return;
4393 }
4394 /* *flag=-1 recoverable error; *flag=-2 unrecoverable error; *flag=0: ok*/
4395
4396 }/* simblkddaskr */
4397 /*--------------------------------------------------------------------------*/
4398 /* grblkddaskr */
grblkddaskr(int * nequations,realtype * tOld,realtype * actual,int * ngc,realtype * res,double * dummy1,int * dummy2)4399 static void grblkddaskr(int *nequations, realtype *tOld, realtype *actual, int *ngc, realtype *res, double *dummy1, int *dummy2)
4400 {
4401 double tx = 0.;
4402 int jj = 0;
4403
4404 tx = (double) * tOld;
4405
4406 *ierr = 0;
4407 C2F(ierode).iero = 0;
4408 zdoit(&tx, actual, actual, res);
4409 C2F(ierode).iero = *ierr;
4410
4411 if (*ierr == 0)
4412 {
4413 /* NaN checking */
4414 for (jj = 0; jj < *ngc; jj++)
4415 {
4416 if (res[jj] - res[jj] != 0)
4417 {
4418 Sciwarning(_("\nWarning: The zero-crossing function #%d returns a NaN"), jj);
4419 return;
4420 }
4421 }
4422 }
4423 else
4424 {
4425 sciprint(_("\nError: Problem in the evaluation of a root function"));
4426 return;
4427 }
4428
4429 }/* grblkddaskr */
4430 /*--------------------------------------------------------------------------*/
4431 /* jacpsol */
jacpsol(realtype * res,int * ires,int * neq,realtype * tOld,realtype * actual,realtype * actualP,realtype * rewt,realtype * savr,realtype * wk,realtype * h,realtype * cj,realtype * wp,int * iwp,int * ier,double * dummy1,int * dummy2)4432 static void jacpsol(realtype *res, int *ires, int *neq, realtype *tOld, realtype *actual, realtype *actualP,
4433 realtype *rewt, realtype *savr, realtype *wk, realtype *h, realtype *cj, realtype *wp, int *iwp,
4434 int *ier, double *dummy1, int *dummy2)
4435 {
4436 /* Here, we compute the system preconditioner matrix P, which is actually the jacobian matrix,
4437 so P(i,j) = dres(i)/dactual(j) + cj*dres(i)/dactualP(j), and we LU-decompose it. */
4438 int i = 0, j = 0, nrow = 0, info = 0;
4439 realtype tx = 0, del = 0, delinv = 0, ysave = 0, ypsave = 0;
4440 realtype * e = NULL;
4441
4442 tx = *tOld;
4443
4444 /* Work array used to evaluate res(*tOld, actual + small_increment, actualP + small_increment).
4445 savr already contains res(*tOld, actual, actualP). */
4446 e = (realtype *) calloc(*neq, sizeof(realtype));
4447
4448 for (i = 0; i < *neq; ++i)
4449 {
4450 del = max (SQuround * max(fabs(actual[i]), fabs(*h * actualP[i])), 1. / rewt[i]);
4451 del *= (*h * actualP[i] >= 0) ? 1 : -1;
4452 del = (actual[i] + del) - actual[i];
4453 ysave = actual[i];
4454 ypsave = actualP[i];
4455 actual[i] += del;
4456 actualP[i] += *cj * del;
4457 *ierr = 0;
4458 C2F(ierode).iero = 0;
4459 simblkddaskr(tOld, actual, actualP, e, ires, dummy1, dummy2);
4460 C2F(ierode).iero = *ierr;
4461 if (*ires == 0)
4462 {
4463 delinv = 1. / del;
4464 for (j = 0; j < *neq; ++j)
4465 {
4466 wp[nrow + j] = (e[j] - savr[j]) * delinv;
4467 /* NaN test */
4468 if (wp[nrow + j] - wp[nrow + j] != 0)
4469 {
4470 Sciwarning(_("\nWarning: The preconditioner evaluation function returns a NaN at index #%d."), nrow + j);
4471 *ier = -1;
4472 }
4473 }
4474 nrow += *neq;
4475 actual[i] = ysave;
4476 actualP[i] = ypsave;
4477 }
4478 else
4479 {
4480 sciprint(_("\nError: The preconditioner evaluation function failed."));
4481 *ier = -1;
4482 free(e);
4483 return;
4484 }
4485 }
4486
4487 /* Proceed to LU factorization of P. */
4488 C2F(dgefa) (wp, neq, neq, iwp, &info);
4489 if (info != 0)
4490 {
4491 sciprint(_("\nError: Failed to factor the preconditioner."));
4492 *ier = -1;
4493 }
4494
4495 free(e);
4496 }/* jacpsol */
4497 /*--------------------------------------------------------------------------*/
4498 /* psol */
psol(int * neq,realtype * tOld,realtype * actual,realtype * actualP,realtype * savr,realtype * wk,realtype * cj,realtype * wght,realtype * wp,int * iwp,realtype * b,realtype * eplin,int * ier,double * dummy1,int * dummy2)4499 static void psol(int *neq, realtype *tOld, realtype *actual, realtype *actualP,
4500 realtype *savr, realtype *wk, realtype *cj, realtype *wght, realtype *wp,
4501 int *iwp, realtype *b, realtype *eplin, int *ier, double *dummy1, int *dummy2)
4502 {
4503 /* This function "applies" the inverse of the preconditioner to 'b' (computes P^-1*b).
4504 It is done by solving P*x = b using the linpack routine 'dgesl'. */
4505 int i = 0, job = 0;
4506
4507 C2F(dgesl) (wp, neq, neq, iwp, b, &job);
4508
4509 /* NaN test */
4510 for (i = 0; i < *neq; ++i)
4511 {
4512 if (b[i] - b[i] != 0)
4513 {
4514 Sciwarning(_("\nWarning: The preconditioner application function returns a NaN at index #%d."), i);
4515 /* Indicate a recoverable error, meaning that the step will be retried with the same step size
4516 but with a call to 'jacpsol' to update necessary data, unless the Jacobian data is current,
4517 in which case the step will be retried with a smaller step size. */
4518 *ier = 1;
4519 }
4520 }
4521 }/* psol */
4522 /*--------------------------------------------------------------------------*/
4523 /* Subroutine addevs */
addevs(double t,int * evtnb,int * ierr1)4524 static void addevs(double t, int *evtnb, int *ierr1)
4525 {
4526 static int i = 0, j = 0;
4527
4528 /* Function Body */
4529 *ierr1 = 0;
4530 if (evtspt[*evtnb] != -1)
4531 {
4532 if ((evtspt[*evtnb] == 0) && (*pointi == *evtnb))
4533 {
4534 tevts[*evtnb] = t;
4535 return;
4536 }
4537 else
4538 {
4539 if (*pointi == *evtnb)
4540 {
4541 *pointi = evtspt[*evtnb]; /* remove from chain */
4542 }
4543 else
4544 {
4545 i = *pointi;
4546 while (*evtnb != evtspt[i])
4547 {
4548 i = evtspt[i];
4549 }
4550 evtspt[i] = evtspt[*evtnb]; /* remove old evtnb from chain */
4551 if (TCritWarning == 0)
4552 {
4553 sciprint(_("\n Warning: an event is reprogrammed at t=%g by removing another"), t );
4554 sciprint(_("\n (already programmed) event. There may be an error in"));
4555 Sciwarning(_("\n your model. Please check your model\n"));
4556 TCritWarning = 1;
4557 }
4558 do_cold_restart(); /* the erased event could be a critical
4559 event, so do_cold_restart is added to
4560 refresh the critical event table */
4561 }
4562 evtspt[*evtnb] = 0;
4563 tevts[*evtnb] = t;
4564 }
4565 }
4566 else
4567 {
4568 evtspt[*evtnb] = 0;
4569 tevts[*evtnb] = t;
4570 }
4571 if (*pointi == 0)
4572 {
4573 *pointi = *evtnb;
4574 return;
4575 }
4576 if (t < tevts[*pointi])
4577 {
4578 evtspt[*evtnb] = *pointi;
4579 *pointi = *evtnb;
4580 return;
4581 }
4582 i = *pointi;
4583
4584 L100:
4585 if (evtspt[i] == 0)
4586 {
4587 evtspt[i] = *evtnb;
4588 return;
4589 }
4590 if (t >= tevts[evtspt[i]])
4591 {
4592 j = evtspt[i];
4593 if (evtspt[j] == 0)
4594 {
4595 evtspt[j] = *evtnb;
4596 return;
4597 }
4598 i = j;
4599 goto L100;
4600 }
4601 else
4602 {
4603 evtspt[*evtnb] = evtspt[i];
4604 evtspt[i] = *evtnb;
4605 }
4606 } /* addevs */
4607 /*--------------------------------------------------------------------------*/
4608 /* Subroutine putevs */
putevs(double * t,int * evtnb,int * ierr1)4609 void putevs(double *t, int *evtnb, int *ierr1)
4610 {
4611 /* Function Body */
4612 *ierr1 = 0;
4613 if (evtspt[*evtnb] != -1)
4614 {
4615 *ierr1 = 1;
4616 return;
4617 }
4618 else
4619 {
4620 evtspt[*evtnb] = 0;
4621 tevts[*evtnb] = *t;
4622 }
4623 if (*pointi == 0)
4624 {
4625 *pointi = *evtnb;
4626 return;
4627 }
4628 evtspt[*evtnb] = *pointi;
4629 *pointi = *evtnb;
4630 } /* putevs */
4631 /*--------------------------------------------------------------------------*/
4632 /* Subroutine idoit */
idoit(double * told)4633 static void idoit(double *told)
4634 {
4635 /* initialisation (propagation of constant blocks outputs) */
4636 /* Copyright INRIA */
4637
4638 int i2 = 0;
4639 scicos_flag flag = 0;
4640 int i = 0, j = 0;
4641 int ierr1 = 0;
4642
4643 ScicosImport *scs_imp = NULL;
4644 int *kf = NULL;
4645
4646 scs_imp = getscicosimportptr();
4647
4648 flag = 1;
4649 for (j = 0; j < * (scs_imp->niord); j++)
4650 {
4651 kf = &scs_imp->iord[j];
4652 C2F(curblk).kfun = *kf; /* */
4653 if (scs_imp->funtyp[*kf - 1] > -1)
4654 {
4655 /* continuous state */
4656 if (scs_imp->blocks[*kf - 1].nx > 0)
4657 {
4658 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
4659 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
4660 }
4661 scs_imp->blocks[*kf - 1].nevprt = scs_imp->iord[j + * (scs_imp->niord)];
4662 /*callf(told, xd, x, x,x,&flag);*/
4663 callf(told, &scs_imp->blocks[*kf - 1], &flag);
4664 if (flag < 0)
4665 {
4666 *ierr = 5 - flag;
4667 return;
4668 }
4669 }
4670 if (scs_imp->blocks[*kf - 1].nevout > 0)
4671 {
4672 if (scs_imp->funtyp[*kf - 1] < 0)
4673 {
4674 i = synchro_nev(scs_imp, *kf, ierr);
4675 if (*ierr != 0)
4676 {
4677 return;
4678 }
4679 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
4680 putevs(told, &i2, &ierr1);
4681 if (ierr1 != 0)
4682 {
4683 /* event conflict */
4684 *ierr = 3;
4685 return;
4686 }
4687 doit(told);
4688 if (*ierr != 0)
4689 {
4690 return;
4691 }
4692 }
4693 }
4694 }
4695 } /* idoit_ */
4696 /*--------------------------------------------------------------------------*/
4697 /* Subroutine doit */
doit(double * told)4698 static void doit(double *told)
4699 {
4700 /* propagation of blocks outputs on discrete activations */
4701 /* Copyright INRIA */
4702
4703 int i = 0, i2 = 0;
4704 scicos_flag flag = 0;
4705 int nord = 0;
4706 int ierr1 = 0;
4707 int ii = 0, kever = 0;
4708
4709 ScicosImport *scs_imp = NULL;
4710 int *kf = NULL;
4711
4712 scs_imp = getscicosimportptr();
4713
4714 kever = *pointi;
4715 *pointi = evtspt[kever];
4716 evtspt[kever] = -1;
4717
4718 nord = scs_imp->ordptr[kever] - scs_imp->ordptr[kever - 1];
4719 if (nord == 0)
4720 {
4721 return;
4722 }
4723
4724 for (ii = scs_imp->ordptr[kever - 1]; ii <= scs_imp->ordptr[kever] - 1 ; ii++)
4725 {
4726 kf = &scs_imp->ordclk[ii - 1];
4727 C2F(curblk).kfun = *kf;
4728 if (scs_imp->funtyp[*kf - 1] > -1)
4729 {
4730 /* continuous state */
4731 if (scs_imp->blocks[*kf - 1].nx > 0)
4732 {
4733 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
4734 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
4735 }
4736 scs_imp->blocks[*kf - 1].nevprt = abs(scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1]);
4737 flag = 1;
4738 /*callf(told, xd, x, x,x,&flag);*/
4739 callf(told, &scs_imp->blocks[*kf - 1], &flag);
4740 if (flag < 0)
4741 {
4742 *ierr = 5 - flag;
4743 return;
4744 }
4745 }
4746
4747 /* Initialize tvec */
4748 if (scs_imp->blocks[*kf - 1].nevout > 0)
4749 {
4750 if (scs_imp->funtyp[*kf - 1] < 0)
4751 {
4752 i = synchro_nev(scs_imp, *kf, ierr);
4753 if (*ierr != 0)
4754 {
4755 return;
4756 }
4757 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
4758 putevs(told, &i2, &ierr1);
4759 if (ierr1 != 0)
4760 {
4761 /* event conflict */
4762 *ierr = 3;
4763 return;
4764 }
4765 doit(told);
4766 if (*ierr != 0)
4767 {
4768 return;
4769 }
4770 }
4771 }
4772 }
4773 } /* doit_ */
4774 /*--------------------------------------------------------------------------*/
4775 /* Subroutine cdoit */
cdoit(double * told)4776 static void cdoit(double *told)
4777 {
4778 /* propagation of continuous blocks outputs */
4779 /* Copyright INRIA */
4780
4781 int i2 = 0;
4782 scicos_flag flag = 0;
4783 int ierr1 = 0;
4784 int i = 0, j = 0;
4785
4786 ScicosImport *scs_imp = NULL;
4787 int *kf = NULL;
4788
4789 scs_imp = getscicosimportptr();
4790
4791 /* Function Body */
4792 for (j = 0; j < * (scs_imp->ncord); j++)
4793 {
4794 kf = &scs_imp->cord[j];
4795 C2F(curblk).kfun = *kf;
4796 /* continuous state */
4797 if (scs_imp->blocks[*kf - 1].nx > 0)
4798 {
4799 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
4800 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
4801 }
4802 scs_imp->blocks[*kf - 1].nevprt = scs_imp->cord[j + * (scs_imp->ncord)];
4803 if (scs_imp->funtyp[*kf - 1] > -1)
4804 {
4805 flag = 1;
4806 /*callf(told, xd, x, x,x,&flag);*/
4807 callf(told, &scs_imp->blocks[*kf - 1], &flag);
4808 if (flag < 0)
4809 {
4810 *ierr = 5 - flag;
4811 return;
4812 }
4813 }
4814
4815 /* Initialize tvec */
4816 if (scs_imp->blocks[*kf - 1].nevout > 0)
4817 {
4818 if (scs_imp->funtyp[*kf - 1] < 0)
4819 {
4820 i = synchro_nev(scs_imp, *kf, ierr);
4821 if (*ierr != 0)
4822 {
4823 return;
4824 }
4825 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
4826 putevs(told, &i2, &ierr1);
4827 if (ierr1 != 0)
4828 {
4829 /* event conflict */
4830 *ierr = 3;
4831 return;
4832 }
4833 doit(told);
4834 if (*ierr != 0)
4835 {
4836 return;
4837 }
4838 }
4839 }
4840 }
4841 } /* cdoit_ */
4842 /*--------------------------------------------------------------------------*/
4843 /* Subroutine ddoit */
ddoit(double * told)4844 static void ddoit(double *told)
4845 {
4846 /* update states & event out on discrete activations */
4847 /* Copyright INRIA */
4848
4849 int i2 = 0, j = 0;
4850 scicos_flag flag = 0;
4851 int kiwa = 0;
4852 int i = 0, i3 = 0, ierr1 = 0;
4853 int ii = 0, keve = 0;
4854
4855 ScicosImport *scs_imp = NULL;
4856 int *kf = NULL;
4857
4858 scs_imp = getscicosimportptr();
4859
4860 /* Function Body */
4861 kiwa = 0;
4862 edoit(told, &kiwa);
4863 if (*ierr != 0)
4864 {
4865 return;
4866 }
4867
4868 /* update continuous and discrete states on event */
4869 if (kiwa == 0)
4870 {
4871 return;
4872 }
4873 for (i = 0; i < kiwa; i++)
4874 {
4875 keve = iwa[i];
4876 if (critev[keve] != 0)
4877 {
4878 hot = 0;
4879 }
4880 i2 = scs_imp->ordptr[keve] - 1;
4881 for (ii = scs_imp->ordptr[keve - 1]; ii <= i2; ii++)
4882 {
4883 kf = &scs_imp->ordclk[ii - 1];
4884 C2F(curblk).kfun = *kf;
4885 /* continuous state */
4886 if (scs_imp->blocks[*kf - 1].nx > 0)
4887 {
4888 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
4889 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
4890 }
4891
4892 scs_imp->blocks[*kf - 1].nevprt = scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1];
4893
4894 if (scs_imp->blocks[*kf - 1].nevout > 0)
4895 {
4896 if (scs_imp->funtyp[*kf - 1] >= 0)
4897 {
4898 /* initialize evout */
4899 for (j = 0; j < scs_imp->blocks[*kf - 1].nevout; j++)
4900 {
4901 scs_imp->blocks[*kf - 1].evout[j] = -1;
4902 }
4903 flag = 3;
4904
4905 if (scs_imp->blocks[*kf - 1].nevprt > 0) /* if event has continuous origin don't call*/
4906 {
4907 /*callf(told, xd, x, x ,x,&flag);*/
4908 callf(told, &scs_imp->blocks[*kf - 1], &flag);
4909 if (flag < 0)
4910 {
4911 *ierr = 5 - flag;
4912 return;
4913 }
4914 }
4915
4916 for (j = 0; j < scs_imp->blocks[*kf - 1].nevout; j++)
4917 {
4918 if (scs_imp->blocks[*kf - 1].evout[j] >= 0.)
4919 {
4920 i3 = j + scs_imp->clkptr[*kf - 1] ;
4921 addevs(scs_imp->blocks[*kf - 1].evout[j] + (*told), &i3, &ierr1);
4922 if (ierr1 != 0)
4923 {
4924 /* event conflict */
4925 *ierr = 3;
4926 return;
4927 }
4928 }
4929 }
4930 }
4931 }
4932
4933 if (scs_imp->blocks[*kf - 1].nevprt > 0)
4934 {
4935 if (scs_imp->blocks[*kf - 1].nx + scs_imp->blocks[*kf - 1].nz + scs_imp->blocks[*kf - 1].noz > 0 || \
4936 *scs_imp->blocks[*kf - 1].work != NULL)
4937 {
4938 /* if a hidden state exists, must also call (for new scope eg) */
4939 /* to avoid calling non-real activations */
4940 flag = 2;
4941 /*callf(told, xd, x, x,x,&flag);*/
4942 callf(told, &scs_imp->blocks[*kf - 1], &flag);
4943 if (flag < 0)
4944 {
4945 *ierr = 5 - flag;
4946 return;
4947 }
4948 }
4949 }
4950 else
4951 {
4952 if (*scs_imp->blocks[*kf - 1].work != NULL)
4953 {
4954 flag = 2;
4955 scs_imp->blocks[*kf - 1].nevprt = 0; /* in case some hidden continuous blocks need updating */
4956 /*callf(told, xd, x, x,x,&flag);*/
4957 callf(told, &scs_imp->blocks[*kf - 1], &flag);
4958 if (flag < 0)
4959 {
4960 *ierr = 5 - flag;
4961 return;
4962 }
4963 }
4964 }
4965 }
4966 }
4967 } /* ddoit_ */
4968 /*--------------------------------------------------------------------------*/
4969 /* Subroutine edoit */
edoit(double * told,int * kiwa)4970 static void edoit(double *told, int *kiwa)
4971 {
4972 /* update blocks output on discrete activations */
4973 /* Copyright INRIA */
4974
4975 int i2 = 0;
4976 scicos_flag flag = 0;
4977 int ierr1 = 0, i = 0;
4978 int kever = 0, ii = 0;
4979
4980 ScicosImport *scs_imp = NULL;
4981 int *kf = NULL;
4982 int nord = 0;
4983
4984 scs_imp = getscicosimportptr();
4985
4986 /* Function Body */
4987 kever = *pointi;
4988
4989 *pointi = evtspt[kever];
4990 evtspt[kever] = -1;
4991
4992 nord = scs_imp->ordptr[kever] - scs_imp->ordptr[kever - 1];
4993 if (nord == 0)
4994 {
4995 return;
4996 }
4997 iwa[*kiwa] = kever;
4998 ++(*kiwa);
4999 for (ii = scs_imp->ordptr[kever - 1]; ii <= scs_imp->ordptr[kever] - 1; ii++)
5000 {
5001 kf = &scs_imp->ordclk[ii - 1];
5002 C2F(curblk).kfun = *kf;
5003
5004 if (scs_imp->funtyp[*kf - 1] > -1)
5005 {
5006 /* continuous state */
5007 if (scs_imp->blocks[*kf - 1].nx > 0)
5008 {
5009 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
5010 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
5011 }
5012
5013 scs_imp->blocks[*kf - 1].nevprt = abs(scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1]);
5014
5015 flag = 1;
5016 /*callf(told, xd, x, x,x,&flag);*/
5017 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5018 if (flag < 0)
5019 {
5020 *ierr = 5 - flag;
5021 return;
5022 }
5023 }
5024
5025 /* Initialize tvec */
5026 if (scs_imp->blocks[*kf - 1].nevout > 0)
5027 {
5028 if (scs_imp->funtyp[*kf - 1] < 0)
5029 {
5030 i = synchro_nev(scs_imp, *kf, ierr);
5031 if (*ierr != 0)
5032 {
5033 return;
5034 }
5035 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
5036 putevs(told, &i2, &ierr1);
5037 if (ierr1 != 0)
5038 {
5039 /* event conflict */
5040 *ierr = 3;
5041 return;
5042 }
5043 edoit(told, kiwa);
5044 if (*ierr != 0)
5045 {
5046 return;
5047 }
5048 }
5049 }
5050 }
5051 } /* edoit_ */
5052 /*--------------------------------------------------------------------------*/
5053 /* Subroutine odoit */
odoit(double * told,double * xt,double * xtd,double * residual)5054 static void odoit(double *told, double *xt, double *xtd, double *residual)
5055 {
5056 /* update blocks derivative of continuous time block */
5057 /* Copyright INRIA */
5058
5059 int i2 = 0;
5060 scicos_flag flag = 0;
5061 int keve = 0, kiwa = 0;
5062 int ierr1 = 0, i = 0;
5063 int ii = 0, jj = 0;
5064
5065 ScicosImport *scs_imp = NULL;
5066 int *kf = NULL;
5067
5068 scs_imp = getscicosimportptr();
5069
5070 /* Function Body */
5071 kiwa = 0;
5072 for (jj = 0; jj < * (scs_imp->noord); jj++)
5073 {
5074 kf = &scs_imp->oord[jj];
5075 C2F(curblk).kfun = *kf;
5076 /* continuous state */
5077 if (scs_imp->blocks[*kf - 1].nx > 0)
5078 {
5079 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5080 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5081 scs_imp->blocks[*kf - 1].res = &residual[scs_imp->xptr[*kf - 1] - 1];
5082 }
5083
5084 scs_imp->blocks[*kf - 1].nevprt = scs_imp->oord[jj + * (scs_imp->noord)];
5085 if (scs_imp->funtyp[*kf - 1] > -1)
5086 {
5087 flag = 1;
5088 /*callf(told, xtd, xt, residual,x,&flag);*/
5089 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5090 if (flag < 0)
5091 {
5092 *ierr = 5 - flag;
5093 return;
5094 }
5095 }
5096
5097 if (scs_imp->blocks[*kf - 1].nevout > 0)
5098 {
5099 if (scs_imp->funtyp[*kf - 1] < 0)
5100 {
5101 if (scs_imp->blocks[*kf - 1].nmode > 0)
5102 {
5103 i2 = scs_imp->blocks[*kf - 1].mode[0] + scs_imp->clkptr[*kf - 1] - 1;
5104 }
5105 else
5106 {
5107 i = synchro_nev(scs_imp, *kf, ierr);
5108 if (*ierr != 0)
5109 {
5110 return;
5111 }
5112 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
5113 }
5114 putevs(told, &i2, &ierr1);
5115 if (ierr1 != 0)
5116 {
5117 /* event conflict */
5118 *ierr = 3;
5119 return;
5120 }
5121 ozdoit(told, xt, xtd, &kiwa);
5122 if (*ierr != 0)
5123 {
5124 return;
5125 }
5126 }
5127 }
5128 }
5129
5130 /* update states derivatives */
5131 for (ii = 0; ii < * (scs_imp->noord); ii++)
5132 {
5133 kf = &scs_imp->oord[ii];
5134 C2F(curblk).kfun = *kf;
5135 if (scs_imp->blocks[*kf - 1].nx > 0 || \
5136 *scs_imp->blocks[*kf - 1].work != NULL)
5137 {
5138 /* work tests if a hidden state exists, used for delay block */
5139 flag = 0;
5140 /* continuous state */
5141 if (scs_imp->blocks[*kf - 1].nx > 0)
5142 {
5143 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5144 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5145 scs_imp->blocks[*kf - 1].res = &residual[scs_imp->xptr[*kf - 1] - 1];
5146 }
5147 scs_imp->blocks[*kf - 1].nevprt = scs_imp->oord[ii + * (scs_imp->noord)];
5148 /*callf(told, xtd, xt, residual,xt,&flag);*/
5149 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5150
5151 if (flag < 0)
5152 {
5153 *ierr = 5 - flag;
5154 return;
5155 }
5156 }
5157 }
5158
5159 for (i = 0; i < kiwa; i++)
5160 {
5161 keve = iwa[i];
5162 for (ii = scs_imp->ordptr[keve - 1]; ii <= scs_imp->ordptr[keve] - 1; ii++)
5163 {
5164 kf = &scs_imp->ordclk[ii - 1];
5165 C2F(curblk).kfun = *kf;
5166 if (scs_imp->blocks[*kf - 1].nx > 0 || \
5167 *scs_imp->blocks[*kf - 1].work != NULL)
5168 {
5169 /* work tests if a hidden state exists */
5170 flag = 0;
5171 /* continuous state */
5172 if (scs_imp->blocks[*kf - 1].nx > 0)
5173 {
5174 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5175 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5176 scs_imp->blocks[*kf - 1].res = &residual[scs_imp->xptr[*kf - 1] - 1];
5177 }
5178 scs_imp->blocks[*kf - 1].nevprt = abs(scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1]);
5179 /*callf(told, xtd, xt, residual,xt,&flag);*/
5180 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5181
5182 if (flag < 0)
5183 {
5184 *ierr = 5 - flag;
5185 return;
5186 }
5187 }
5188 }
5189 }
5190 } /* odoit_ */
5191 /*--------------------------------------------------------------------------*/
5192 /* Subroutine reinitdoit */
reinitdoit(double * told)5193 static void reinitdoit(double *told)
5194 {
5195 /* update blocks xproperties of continuous time block */
5196 /* Copyright INRIA */
5197
5198 int i2 = 0;
5199 scicos_flag flag = 0;
5200 int keve = 0, kiwa = 0;
5201 int ierr1 = 0, i = 0;
5202 int ii = 0, jj = 0;
5203
5204 ScicosImport *scs_imp = NULL;
5205 int *kf = NULL;
5206
5207 scs_imp = getscicosimportptr();
5208
5209 /* Function Body */
5210 kiwa = 0;
5211 for (jj = 0; jj < * (scs_imp->noord); jj++)
5212 {
5213 kf = &scs_imp->oord[jj];
5214 C2F(curblk).kfun = *kf;
5215 /* continuous state */
5216 if (scs_imp->blocks[*kf - 1].nx > 0)
5217 {
5218 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
5219 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
5220 }
5221 scs_imp->blocks[*kf - 1].nevprt = scs_imp->oord[jj + * (scs_imp->noord)];
5222 if (scs_imp->funtyp[*kf - 1] > -1)
5223 {
5224 flag = 1;
5225 /*callf(told, xd, x, x,x,&flag);*/
5226 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5227 if (flag < 0)
5228 {
5229 *ierr = 5 - flag;
5230 return;
5231 }
5232 }
5233
5234 if (scs_imp->blocks[*kf - 1].nevout > 0 && scs_imp->funtyp[*kf - 1] < 0)
5235 {
5236 i = synchro_nev(scs_imp, *kf, ierr);
5237 if (*ierr != 0)
5238 {
5239 return;
5240 }
5241 if (scs_imp->blocks[*kf - 1].nmode > 0)
5242 {
5243 scs_imp->blocks[*kf - 1].mode[0] = i;
5244 }
5245 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
5246 putevs(told, &i2, &ierr1);
5247 if (ierr1 != 0)
5248 {
5249 /* event conflict */
5250 *ierr = 3;
5251 return;
5252 }
5253 doit(told);
5254 if (*ierr != 0)
5255 {
5256 return;
5257 }
5258 }
5259 }
5260
5261 /* reinitialize */
5262 for (ii = 0; ii < * (scs_imp->noord); ii++)
5263 {
5264 kf = &scs_imp->oord[ii];
5265 C2F(curblk).kfun = *kf;
5266 if (scs_imp->blocks[*kf - 1].nx > 0)
5267 {
5268 flag = 7;
5269 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
5270 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
5271 scs_imp->blocks[*kf - 1].res = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
5272 scs_imp->blocks[*kf - 1].nevprt = scs_imp->oord[ii + * (scs_imp->noord)];
5273 scs_imp->blocks[*kf - 1].xprop = &scs_imp->xprop[-1 + scs_imp->xptr[*kf - 1]];
5274 /*callf(told, xd, x, xd,x,&flag);*/
5275 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5276 if (flag < 0)
5277 {
5278 *ierr = 5 - flag;
5279 return;
5280 }
5281 }
5282 }
5283
5284 for (i = 0; i < kiwa; i++)
5285 {
5286 keve = iwa[i];
5287 for (ii = scs_imp->ordptr[keve - 1]; ii <= scs_imp->ordptr[keve] - 1; ii++)
5288 {
5289 kf = &scs_imp->ordclk[ii - 1];
5290 C2F(curblk).kfun = *kf;
5291 if (scs_imp->blocks[*kf - 1].nx > 0)
5292 {
5293 flag = 7;
5294 scs_imp->blocks[*kf - 1].x = &scs_imp->x[scs_imp->xptr[*kf - 1] - 1];
5295 scs_imp->blocks[*kf - 1].xd = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
5296 scs_imp->blocks[*kf - 1].res = &scs_imp->xd[scs_imp->xptr[*kf - 1] - 1];
5297 scs_imp->blocks[*kf - 1].nevprt = abs(scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1]);
5298 scs_imp->blocks[*kf - 1].xprop = &scs_imp->xprop[-1 + scs_imp->xptr[*kf - 1]];
5299 /*callf(told, xd, x, xd,x,&flag);*/
5300 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5301 if (flag < 0)
5302 {
5303 *ierr = 5 - flag;
5304 return;
5305 }
5306 }
5307 }
5308 }
5309 } /* reinitdoit_ */
5310 /*--------------------------------------------------------------------------*/
5311 /* Subroutine ozdoit */
ozdoit(double * told,double * xt,double * xtd,int * kiwa)5312 static void ozdoit(double *told, double *xt, double *xtd, int *kiwa)
5313 {
5314 /* update blocks output of continuous time block on discrete activations */
5315 /* Copyright INRIA */
5316
5317 int i2 = 0;
5318 scicos_flag flag = 0;
5319 int nord = 0;
5320 int ierr1 = 0, i = 0;
5321 int ii = 0, kever = 0;
5322
5323 ScicosImport *scs_imp = NULL;
5324 int *kf = NULL;
5325
5326 scs_imp = getscicosimportptr();
5327
5328 /* Function Body */
5329 kever = *pointi;
5330 *pointi = evtspt[kever];
5331 evtspt[kever] = -1;
5332
5333 nord = scs_imp->ordptr[kever] - scs_imp->ordptr[kever - 1];
5334 if (nord == 0)
5335 {
5336 return;
5337 }
5338 iwa[*kiwa] = kever;
5339 ++(*kiwa);
5340
5341 for (ii = scs_imp->ordptr[kever - 1]; ii <= scs_imp->ordptr[kever] - 1; ii++)
5342 {
5343 kf = &scs_imp->ordclk[ii - 1];
5344 C2F(curblk).kfun = *kf;
5345 if (scs_imp->funtyp[*kf - 1] > -1)
5346 {
5347 /* continuous state */
5348 if (scs_imp->blocks[*kf - 1].nx > 0)
5349 {
5350 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5351 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5352 }
5353 scs_imp->blocks[*kf - 1].nevprt = abs(scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1]);
5354 flag = 1;
5355 /*callf(told, xtd, xt, xt,x,&flag);*/
5356 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5357 if (flag < 0)
5358 {
5359 *ierr = 5 - flag;
5360 return;
5361 }
5362 }
5363
5364 /* Initialize tvec */
5365 if (scs_imp->blocks[*kf - 1].nevout > 0)
5366 {
5367 if (scs_imp->funtyp[*kf - 1] < 0)
5368 {
5369 if (phase == 1 || scs_imp->blocks[*kf - 1].nmode == 0)
5370 {
5371 i = synchro_nev(scs_imp, *kf, ierr);
5372 if (*ierr != 0)
5373 {
5374 return;
5375 }
5376 }
5377 else
5378 {
5379 i = scs_imp->blocks[*kf - 1].mode[0];
5380 }
5381 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
5382 putevs(told, &i2, &ierr1);
5383 if (ierr1 != 0)
5384 {
5385 /* event conflict */
5386 *ierr = 3;
5387 return;
5388 }
5389 ozdoit(told, xt, xtd, kiwa);
5390 }
5391 }
5392 }
5393 } /* ozdoit_ */
5394 /*--------------------------------------------------------------------------*/
5395 /* Subroutine zdoit */
zdoit(double * told,double * xt,double * xtd,double * g)5396 static void zdoit(double *told, double *xt, double *xtd, double *g)
5397 {
5398 /* update blocks zcross of continuous time block */
5399 /* Copyright INRIA */
5400 int i2 = 0;
5401 scicos_flag flag = 0;
5402 int keve = 0, kiwa = 0;
5403 int ierr1 = 0, i = 0, j = 0;
5404 int ii = 0, jj = 0;
5405
5406 ScicosImport *scs_imp = NULL;
5407 int *kf = NULL;
5408
5409 scs_imp = getscicosimportptr();
5410
5411 /* Function Body */
5412 for (i = 0; i < * (scs_imp->ng); i++)
5413 {
5414 g[i] = 0.;
5415 }
5416
5417 kiwa = 0;
5418 for (jj = 0; jj < * (scs_imp->nzord); jj++)
5419 {
5420 kf = &scs_imp->zord[jj];
5421 C2F(curblk).kfun = *kf;
5422 /* continuous state */
5423 if (scs_imp->blocks[*kf - 1].nx > 0)
5424 {
5425 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5426 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5427 }
5428 scs_imp->blocks[*kf - 1].nevprt = scs_imp->zord[jj + * (scs_imp->nzord)];
5429
5430 if (scs_imp->funtyp[*kf - 1] > -1)
5431 {
5432 flag = 1;
5433 /*callf(told, xtd, xt, xt,xt,&flag);*/
5434 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5435 if (flag < 0)
5436 {
5437 *ierr = 5 - flag;
5438 return;
5439 }
5440 }
5441
5442 /* Initialize tvec */
5443 if (scs_imp->blocks[*kf - 1].nevout > 0)
5444 {
5445 if (scs_imp->funtyp[*kf - 1] < 0)
5446 {
5447 if (phase == 1 || scs_imp->blocks[*kf - 1].nmode == 0)
5448 {
5449 i = synchro_nev(scs_imp, *kf, ierr);
5450 if (*ierr != 0)
5451 {
5452 return;
5453 }
5454 }
5455 else
5456 {
5457 i = scs_imp->blocks[*kf - 1].mode[0];
5458 }
5459 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
5460 putevs(told, &i2, &ierr1);
5461 if (ierr1 != 0)
5462 {
5463 /* event conflict */
5464 *ierr = 3;
5465 return;
5466 }
5467 ozdoit(told, xt, xtd, &kiwa);
5468 if (*ierr != 0)
5469 {
5470 return;
5471 }
5472 }
5473 }
5474 }
5475
5476 /* update zero crossing surfaces */
5477 for (ii = 0; ii < * (scs_imp->nzord); ii++)
5478 {
5479 kf = &scs_imp->zord[ii];
5480 C2F(curblk).kfun = *kf;
5481 if (scs_imp->blocks[*kf - 1].ng > 0)
5482 {
5483 /* update g array ptr */
5484 scs_imp->blocks[*kf - 1].g = &g[scs_imp->zcptr[*kf - 1] - 1];
5485 if (scs_imp->funtyp[*kf - 1] > 0)
5486 {
5487 flag = 9;
5488 /* continuous state */
5489 if (scs_imp->blocks[*kf - 1].nx > 0)
5490 {
5491 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5492 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5493 }
5494 scs_imp->blocks[*kf - 1].nevprt = scs_imp->zord[ii + * (scs_imp->nzord)];
5495 /*callf(told, xtd, xt, xtd,g,&flag);*/
5496 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5497 if (flag < 0)
5498 {
5499 *ierr = 5 - flag;
5500 return;
5501 }
5502 }
5503 else
5504 {
5505 j = synchro_g_nev(scs_imp, g, *kf, ierr);
5506 if (*ierr != 0)
5507 {
5508 return;
5509 }
5510 if ( (phase == 1) && (scs_imp->blocks[*kf - 1].nmode > 0) )
5511 {
5512 scs_imp->blocks[*kf - 1].mode[0] = j;
5513 }
5514 }
5515
5516 // scs_imp->blocks[*kf-1].g = &scs_imp->g[scs_imp->zcptr[*kf]-1];
5517
5518 }
5519 }
5520
5521 for (i = 0; i < kiwa; i++)
5522 {
5523 keve = iwa[i];
5524 for (ii = scs_imp->ordptr[keve - 1]; ii <= scs_imp->ordptr[keve] - 1; ii++)
5525 {
5526 kf = &scs_imp->ordclk[ii - 1];
5527 C2F(curblk).kfun = *kf;
5528 if (scs_imp->blocks[*kf - 1].ng > 0)
5529 {
5530 /* update g array ptr */
5531 scs_imp->blocks[*kf - 1].g = &g[scs_imp->zcptr[*kf - 1] - 1];
5532 if (scs_imp->funtyp[*kf - 1] > 0)
5533 {
5534 flag = 9;
5535 /* continuous state */
5536 if (scs_imp->blocks[*kf - 1].nx > 0)
5537 {
5538 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5539 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5540 }
5541 scs_imp->blocks[*kf - 1].nevprt = abs(scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1]);
5542 /*callf(told, xtd, xt, xtd,g,&flag);*/
5543 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5544 if (flag < 0)
5545 {
5546 *ierr = 5 - flag;
5547 return;
5548 }
5549 }
5550 else
5551 {
5552 j = synchro_g_nev(scs_imp, g, *kf, ierr);
5553 if (*ierr != 0)
5554 {
5555 return;
5556 }
5557 if ((phase == 1) && (scs_imp->blocks[*kf - 1].nmode > 0))
5558 {
5559 scs_imp->blocks[*kf - 1].mode[0] = j;
5560 }
5561 }
5562
5563 //scs_imp->blocks[*kf-1].g = &scs_imp->g[scs_imp->zcptr[*kf]-1];
5564 }
5565 }
5566 }
5567 } /* zdoit_ */
5568 /*--------------------------------------------------------------------------*/
5569 /* Subroutine Jdoit */
Jdoit(double * told,double * xt,double * xtd,double * residual,int * job)5570 void Jdoit(double *told, double *xt, double *xtd, double *residual, int *job)
5571 {
5572 /* update blocks jacobian of continuous time block */
5573 /* Copyright INRIA */
5574
5575 int i2 = 0;
5576 scicos_flag flag = 0;
5577 int keve = 0, kiwa = 0;
5578 int ierr1 = 0, i = 0;
5579 int ii = 0, jj = 0;
5580
5581 ScicosImport *scs_imp = NULL;
5582 int *kf = NULL;
5583
5584 scs_imp = getscicosimportptr();
5585
5586 /* Function Body */
5587 kiwa = 0;
5588 for (jj = 0; jj < * (scs_imp->noord); jj++)
5589 {
5590 kf = &scs_imp->oord[jj];
5591 C2F(curblk).kfun = *kf;
5592 scs_imp->blocks[*kf - 1].nevprt = scs_imp->oord[jj + * (scs_imp->noord)];
5593 if (scs_imp->funtyp[*kf - 1] > -1)
5594 {
5595 flag = 1;
5596 /* applying desired output */
5597 if ((*job == 2) && (scs_imp->oord[jj] == AJacobian_block))
5598 {
5599 }
5600 else
5601 /* continuous state */
5602 if (scs_imp->blocks[*kf - 1].nx > 0)
5603 {
5604 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5605 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5606 scs_imp->blocks[*kf - 1].res = &residual[scs_imp->xptr[*kf - 1] - 1];
5607 }
5608
5609 /*callf(told, xtd, xt, residual,x,&flag);*/
5610 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5611 if (flag < 0)
5612 {
5613 *ierr = 5 - flag;
5614 return;
5615 }
5616 }
5617
5618 if (scs_imp->blocks[*kf - 1].nevout > 0)
5619 {
5620 if (scs_imp->funtyp[*kf - 1] < 0)
5621 {
5622 if (scs_imp->blocks[*kf - 1].nmode > 0)
5623 {
5624 i2 = scs_imp->blocks[*kf - 1].mode[0] + scs_imp->clkptr[*kf - 1] - 1;
5625 }
5626 else
5627 {
5628 i = synchro_nev(scs_imp, *kf, ierr);
5629 if (*ierr != 0)
5630 {
5631 return;
5632 }
5633 i2 = i + scs_imp->clkptr[*kf - 1] - 1;
5634 }
5635 putevs(told, &i2, &ierr1);
5636 if (ierr1 != 0)
5637 {
5638 /* event conflict */
5639 *ierr = 3;
5640 return;
5641 }
5642 ozdoit(told, xt, xtd, &kiwa);
5643 }
5644 }
5645 }
5646
5647 /* update states derivatives */
5648 for (ii = 0; ii < * (scs_imp->noord); ii++)
5649 {
5650 kf = &scs_imp->oord[ii];
5651 C2F(curblk).kfun = *kf;
5652 if (scs_imp->blocks[*kf - 1].nx > 0 || \
5653 *scs_imp->blocks[*kf - 1].work != NULL)
5654 {
5655 /* work tests if a hidden state exists, used for delay block */
5656 flag = 0;
5657 if (((*job == 1) && (scs_imp->oord[ii] == AJacobian_block)) || (*job != 1))
5658 {
5659 if (*job == 1)
5660 {
5661 flag = 10;
5662 }
5663 /* continuous state */
5664 if (scs_imp->blocks[*kf - 1].nx > 0)
5665 {
5666 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5667 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5668 scs_imp->blocks[*kf - 1].res = &residual[scs_imp->xptr[*kf - 1] - 1];
5669 }
5670 scs_imp->blocks[*kf - 1].nevprt = scs_imp->oord[ii + * (scs_imp->noord)];
5671 /*callf(told, xtd, xt, residual,xt,&flag);*/
5672 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5673 }
5674 if (flag < 0)
5675 {
5676 *ierr = 5 - flag;
5677 return;
5678 }
5679 }
5680 }
5681
5682 for (i = 0; i < kiwa; i++)
5683 {
5684 keve = iwa[i];
5685 for (ii = scs_imp->ordptr[keve - 1]; ii <= scs_imp->ordptr[keve] - 1; ii++)
5686 {
5687 kf = &scs_imp->ordclk[ii - 1];
5688 C2F(curblk).kfun = *kf;
5689 if (scs_imp->blocks[*kf - 1].nx > 0 || \
5690 *scs_imp->blocks[*kf - 1].work != NULL)
5691 {
5692 /* work tests if a hidden state exists */
5693 flag = 0;
5694 if (((*job == 1) && (scs_imp->oord[ii - 1] == AJacobian_block)) || (*job != 1))
5695 {
5696 if (*job == 1)
5697 {
5698 flag = 10;
5699 }
5700 /* continuous state */
5701 if (scs_imp->blocks[*kf - 1].nx > 0)
5702 {
5703 scs_imp->blocks[*kf - 1].x = &xt[scs_imp->xptr[*kf - 1] - 1];
5704 scs_imp->blocks[*kf - 1].xd = &xtd[scs_imp->xptr[*kf - 1] - 1];
5705 scs_imp->blocks[*kf - 1].res = &residual[scs_imp->xptr[*kf - 1] - 1];
5706 }
5707 scs_imp->blocks[*kf - 1].nevprt = abs(scs_imp->ordclk[ii + * (scs_imp->nordclk) - 1]);
5708 /*callf(told, xtd, xt, residual,xt,&flag);*/
5709 callf(told, &scs_imp->blocks[*kf - 1], &flag);
5710 }
5711 if (flag < 0)
5712 {
5713 *ierr = 5 - flag;
5714 return;
5715 }
5716 }
5717 }
5718 }
5719 } /* Jdoit_ */
5720 /*--------------------------------------------------------------------------*/
5721 /* Subroutine synchro_nev */
synchro_nev(ScicosImport * scs_imp,int kf,int * ierr)5722 static int synchro_nev(ScicosImport *scs_imp, int kf, int *ierr)
5723 {
5724 /* synchro blocks computation */
5725 /* Copyright INRIA */
5726 SCSREAL_COP *outtbdptr = NULL; /*to store double of outtb*/
5727 SCSINT8_COP *outtbcptr = NULL; /*to store int8 of outtb*/
5728 SCSINT16_COP *outtbsptr = NULL; /*to store int16 of outtb*/
5729 SCSINT32_COP *outtblptr = NULL; /*to store int32 of outtb*/
5730 SCSUINT8_COP *outtbucptr = NULL; /*to store unsigned int8 of outtb */
5731 SCSUINT16_COP *outtbusptr = NULL; /*to store unsigned int16 of outtb */
5732 SCSUINT32_COP *outtbulptr = NULL; /*to store unsigned int32 of outtb */
5733
5734 int cond = 0;
5735 int i = 0; /* return 0 by default */
5736
5737 /* variable for param */
5738 int *outtbtyp = 0;
5739 void **outtbptr = NULL;
5740 int *funtyp = 0;
5741 int *inplnk = 0;
5742 int *inpptr = 0;
5743
5744 /* get param ptr */
5745 outtbtyp = scs_imp->outtbtyp;
5746 outtbptr = scs_imp->outtbptr;
5747 funtyp = scs_imp->funtyp;
5748 inplnk = scs_imp->inplnk;
5749 inpptr = scs_imp->inpptr;
5750
5751 /* if-then-else blk */
5752 if (funtyp[kf - 1] == -1)
5753 {
5754 switch (outtbtyp[-1 + inplnk[inpptr[kf - 1] - 1]])
5755 {
5756 case SCSREAL_N :
5757 outtbdptr = (SCSREAL_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5758 cond = (*outtbdptr <= 0.);
5759 break;
5760
5761 case SCSCOMPLEX_N :
5762 outtbdptr = (SCSCOMPLEX_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5763 cond = (*outtbdptr <= 0.);
5764 break;
5765
5766 case SCSINT8_N :
5767 outtbcptr = (SCSINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5768 cond = (*outtbcptr <= 0);
5769 break;
5770
5771 case SCSINT16_N :
5772 outtbsptr = (SCSINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5773 cond = (*outtbsptr <= 0);
5774 break;
5775
5776 case SCSINT32_N :
5777 outtblptr = (SCSINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5778 cond = (*outtblptr <= 0);
5779 break;
5780
5781 case SCSUINT8_N :
5782 outtbucptr = (SCSUINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5783 cond = (*outtbucptr <= 0);
5784 break;
5785
5786 case SCSUINT16_N :
5787 outtbusptr = (SCSUINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5788 cond = (*outtbusptr <= 0);
5789 break;
5790
5791 case SCSUINT32_N :
5792 outtbulptr = (SCSUINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5793 cond = (*outtbulptr <= 0);
5794 break;
5795
5796 default : /* Add a message here */
5797 *ierr = 25;
5798 return 0;
5799 }
5800 if (cond)
5801 {
5802 i = 2;
5803 }
5804 else
5805 {
5806 i = 1;
5807 }
5808 }
5809 /* eselect blk */
5810 else if (funtyp[kf - 1] == -2)
5811 {
5812 switch (outtbtyp[-1 + inplnk[inpptr[kf - 1] - 1]])
5813 {
5814 case SCSREAL_N :
5815 outtbdptr = (SCSREAL_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5816 i = Max(Min((int) * outtbdptr, scs_imp->blocks[kf - 1].nevout), 1);
5817 break;
5818
5819 case SCSCOMPLEX_N :
5820 outtbdptr = (SCSCOMPLEX_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5821 i = Max(Min((int) * outtbdptr, scs_imp->blocks[kf - 1].nevout), 1);
5822 break;
5823
5824 case SCSINT8_N :
5825 outtbcptr = (SCSINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5826 i = Max(Min((int) * outtbcptr,
5827 scs_imp->blocks[kf - 1].nevout), 1);
5828 break;
5829
5830 case SCSINT16_N :
5831 outtbsptr = (SCSINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5832 i = Max(Min((int) * outtbsptr, scs_imp->blocks[kf - 1].nevout), 1);
5833 break;
5834
5835 case SCSINT32_N :
5836 outtblptr = (SCSINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5837 i = Max(Min((int) * outtblptr, scs_imp->blocks[kf - 1].nevout), 1);
5838 break;
5839
5840 case SCSUINT8_N :
5841 outtbucptr = (SCSUINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5842 i = Max(Min((int) * outtbucptr, scs_imp->blocks[kf - 1].nevout), 1);
5843 break;
5844
5845 case SCSUINT16_N :
5846 outtbusptr = (SCSUINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5847 i = Max(Min((int) * outtbusptr, scs_imp->blocks[kf - 1].nevout), 1);
5848 break;
5849
5850 case SCSUINT32_N :
5851 outtbulptr = (SCSUINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5852 i = Max(Min((int) * outtbulptr, scs_imp->blocks[kf - 1].nevout), 1);
5853 break;
5854
5855 default : /* Add a message here */
5856 *ierr = 25;
5857 return 0;
5858 }
5859 }
5860 return i;
5861 } /* synchro_nev */
5862 /*--------------------------------------------------------------------------*/
5863 /* Subroutine synchro_g_nev */
synchro_g_nev(ScicosImport * scs_imp,double * g,int kf,int * ierr)5864 static int synchro_g_nev(ScicosImport *scs_imp, double *g, int kf, int *ierr)
5865 {
5866 /* synchro blocks with zcross computation */
5867 /* Copyright INRIA */
5868 SCSREAL_COP *outtbdptr = NULL; /*to store double of outtb*/
5869 SCSINT8_COP *outtbcptr = NULL; /*to store int8 of outtb*/
5870 SCSINT16_COP *outtbsptr = NULL; /*to store int16 of outtb*/
5871 SCSINT32_COP *outtblptr = NULL; /*to store int32 of outtb*/
5872 SCSUINT8_COP *outtbucptr = NULL; /*to store unsigned int8 of outtb */
5873 SCSUINT16_COP *outtbusptr = NULL; /*to store unsigned int16 of outtb */
5874 SCSUINT32_COP *outtbulptr = NULL; /*to store unsigned int32 of outtb */
5875
5876 int cond = 0;
5877 int i = 0; /* return 0 by default */
5878 int jj = 0;
5879
5880 /* variable for param */
5881 int *outtbtyp = NULL;
5882 void **outtbptr = NULL;
5883 int *funtyp = NULL;
5884 int *inplnk = NULL;
5885 int *inpptr = NULL;
5886 int *zcptr = NULL;
5887
5888 /* get param ptr */
5889 outtbtyp = scs_imp->outtbtyp;
5890 outtbptr = scs_imp->outtbptr;
5891 funtyp = scs_imp->funtyp;
5892 inplnk = scs_imp->inplnk;
5893 inpptr = scs_imp->inpptr;
5894 zcptr = scs_imp->zcptr;
5895
5896 /* if-then-else blk */
5897 if (funtyp[kf - 1] == -1)
5898 {
5899 switch (outtbtyp[-1 + inplnk[inpptr[kf - 1] - 1]])
5900 {
5901 case SCSREAL_N :
5902 outtbdptr = (SCSREAL_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5903 g[zcptr[kf - 1] - 1] = *outtbdptr;
5904 cond = (*outtbdptr <= 0.);
5905 break;
5906
5907 case SCSCOMPLEX_N :
5908 outtbdptr = (SCSCOMPLEX_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5909 g[zcptr[kf - 1] - 1] = *outtbdptr;
5910 cond = (*outtbdptr <= 0.);
5911 break;
5912
5913 case SCSINT8_N :
5914 outtbcptr = (SCSINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5915 g[zcptr[kf - 1] - 1] = (double) * outtbcptr;
5916 cond = (*outtbcptr <= 0);
5917 break;
5918
5919 case SCSINT16_N :
5920 outtbsptr = (SCSINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5921 g[zcptr[kf - 1] - 1] = (double) * outtbsptr;
5922 cond = (*outtbsptr <= 0);
5923 break;
5924
5925 case SCSINT32_N :
5926 outtblptr = (SCSINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5927 g[zcptr[kf - 1] - 1] = (double) * outtblptr;
5928 cond = (*outtblptr <= 0);
5929 break;
5930
5931 case SCSUINT8_N :
5932 outtbucptr = (SCSUINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5933 g[zcptr[kf - 1] - 1] = (double) * outtbucptr;
5934 cond = (*outtbucptr <= 0);
5935 break;
5936
5937 case SCSUINT16_N :
5938 outtbusptr = (SCSUINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5939 g[zcptr[kf - 1] - 1] = (double) * outtbusptr;
5940 cond = (*outtbusptr <= 0);
5941 break;
5942
5943 case SCSUINT32_N :
5944 outtbulptr = (SCSUINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5945 g[zcptr[kf - 1] - 1] = (double) * outtbulptr;
5946 cond = (*outtbulptr <= 0);
5947 break;
5948
5949 default : /* Add a message here */
5950 *ierr = 25;
5951 return 0;
5952 }
5953 if (cond)
5954 {
5955 i = 2;
5956 }
5957 else
5958 {
5959 i = 1;
5960 }
5961 }
5962 /* eselect blk */
5963 else if (funtyp[kf - 1] == -2)
5964 {
5965 switch (outtbtyp[-1 + inplnk[inpptr[kf - 1] - 1]])
5966 {
5967 case SCSREAL_N :
5968 outtbdptr = (SCSREAL_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5969 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
5970 {
5971 g[zcptr[kf - 1] - 1 + jj] = *outtbdptr - (double)(jj + 2);
5972 }
5973 i = Max(Min((int) * outtbdptr, scs_imp->blocks[kf - 1].nevout), 1);
5974 break;
5975
5976 case SCSCOMPLEX_N :
5977 outtbdptr = (SCSCOMPLEX_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5978 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
5979 {
5980 g[zcptr[kf - 1] - 1 + jj] = *outtbdptr - (double)(jj + 2);
5981 }
5982 i = Max(Min((int) * outtbdptr, scs_imp->blocks[kf - 1].nevout), 1);
5983 break;
5984
5985 case SCSINT8_N :
5986 outtbcptr = (SCSINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5987 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
5988 {
5989 g[zcptr[kf - 1] - 1 + jj] = (double) * outtbcptr - (double)(jj + 2);
5990 }
5991 i = Max(Min((int) * outtbcptr, scs_imp->blocks[kf - 1].nevout), 1);
5992 break;
5993
5994 case SCSINT16_N :
5995 outtbsptr = (SCSINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
5996 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
5997 {
5998 g[zcptr[kf - 1] - 1 + jj] = (double) * outtbsptr - (double)(jj + 2);
5999 }
6000 i = Max(Min((int) * outtbsptr, scs_imp->blocks[kf - 1].nevout), 1);
6001 break;
6002
6003 case SCSINT32_N :
6004 outtblptr = (SCSINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
6005 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
6006 {
6007 g[zcptr[kf - 1] - 1 + jj] = (double) * outtblptr - (double)(jj + 2);
6008 }
6009 i = Max(Min((int) * outtblptr, scs_imp->blocks[kf - 1].nevout), 1);
6010 break;
6011
6012 case SCSUINT8_N :
6013 outtbucptr = (SCSUINT8_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
6014 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
6015 {
6016 g[zcptr[kf - 1] - 1 + jj] = (double) * outtbucptr - (double)(jj + 2);
6017 }
6018 i = Max(Min((int) * outtbucptr, scs_imp->blocks[kf - 1].nevout), 1);
6019 break;
6020
6021 case SCSUINT16_N :
6022 outtbusptr = (SCSUINT16_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
6023 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
6024 {
6025 g[zcptr[kf - 1] - 1 + jj] = (double) * outtbusptr - (double)(jj + 2);
6026 }
6027 i = Max(Min((int) * outtbusptr, scs_imp->blocks[kf - 1].nevout), 1);
6028 break;
6029
6030 case SCSUINT32_N :
6031 outtbulptr = (SCSUINT32_COP *)outtbptr[-1 + inplnk[inpptr[kf - 1] - 1]];
6032 for (jj = 0; jj < scs_imp->blocks[kf - 1].nevout - 1; jj++)
6033 {
6034 g[zcptr[kf - 1] - 1 + jj] = (double) * outtbulptr - (double)(jj + 2);
6035 }
6036 i = Max(Min((int) * outtbulptr, scs_imp->blocks[kf - 1].nevout), 1);
6037 break;
6038
6039 default : /* Add a message here */
6040 *ierr = 25;
6041 return 0;
6042 }
6043 }
6044 return i;
6045 } /* synchro_g_nev */
6046 /*--------------------------------------------------------------------------*/
6047 /* FREE_blocks */
FREE_blocks()6048 static void FREE_blocks()
6049 {
6050 int kf = 0;
6051 for (kf = 0; kf < nblk; ++kf)
6052 {
6053 if (Blocks[kf].insz != NULL)
6054 {
6055 FREE(Blocks[kf].insz);
6056 }
6057 if (Blocks[kf].inptr != NULL)
6058 {
6059 FREE(Blocks[kf].inptr);
6060 }
6061 if (Blocks[kf].outsz != NULL)
6062 {
6063 FREE(Blocks[kf].outsz);
6064 }
6065 if (Blocks[kf].outptr != NULL)
6066 {
6067 FREE(Blocks[kf].outptr);
6068 }
6069 if (Blocks[kf].oparsz != NULL)
6070 {
6071 FREE(Blocks[kf].oparsz);
6072 }
6073 if (Blocks[kf].ozsz != NULL)
6074 {
6075 FREE(Blocks[kf].ozsz);
6076 }
6077 if (Blocks[kf].label != NULL)
6078 {
6079 FREE(Blocks[kf].label);
6080 }
6081 if (Blocks[kf].uid != NULL)
6082 {
6083 FREE(Blocks[kf].uid);
6084 }
6085 if (Blocks[kf].evout != NULL)
6086 {
6087 FREE(Blocks[kf].evout);
6088 }
6089 }
6090 FREE(Blocks);
6091
6092 if (nx > 0)
6093 {
6094 FREE(xprop);
6095 }
6096
6097 if (nmod > 0)
6098 {
6099 FREE(mod);
6100 }
6101
6102 if (ng > 0)
6103 {
6104 FREE(g);
6105 }
6106
6107 return;
6108 } /* FREE_blocks */
6109 /*--------------------------------------------------------------------------*/
6110 /* Subroutine funnum2 */
funnum2(char * fname)6111 void* funnum2(char * fname)
6112 {
6113 int i = 0;
6114 while (tabsim[i].name != (char *)NULL)
6115 {
6116 if (strcmp(fname, tabsim[i].name) == 0)
6117 {
6118 return (void*)tabsim[i].fonc;
6119 }
6120 i++;
6121 }
6122 return NULL;
6123 }/* funnum2 */
6124 /*--------------------------------------------------------------------------*/
get_phase_simulation(void)6125 int get_phase_simulation(void)
6126 {
6127 return phase;
6128 }
6129 /*--------------------------------------------------------------------------*/
do_cold_restart(void)6130 void do_cold_restart(void)
6131 {
6132 hot = 0;
6133 return;
6134 }
6135 /*--------------------------------------------------------------------------*/
6136 /* get_scicos_time : return the current
6137 * simulation time
6138 */
get_scicos_time(void)6139 double get_scicos_time(void)
6140 {
6141 return scicos_time;
6142 }
6143 /*--------------------------------------------------------------------------*/
6144 /*! \brief set the current simulation time before calling blocks
6145 *
6146 * As some of the blocks call get_scicos_time(), this is the only way to force
6147 * a local time for these blocks. This call does not modify the Xcos solver
6148 * time but is only used to step to a future point while calling blocks.
6149 */
set_scicos_time(double t)6150 void set_scicos_time(double t)
6151 {
6152 scicos_time = t;
6153 }
6154 /*--------------------------------------------------------------------------*/
6155 /* get_block_number : return the current
6156 * block number
6157 */
get_block_number(void)6158 int get_block_number(void)
6159 {
6160 return C2F(curblk).kfun;
6161 }
6162 /*--------------------------------------------------------------------------*/
6163 /* set_block_error : set an error number
6164 * for block_error
6165 */
set_block_error(int err)6166 void set_block_error(int err)
6167 {
6168 *block_error = err;
6169 }
6170 /*--------------------------------------------------------------------------*/
6171 /* Coserror : copy an error message
6172 * in coserr.buf an set block_error to
6173 * -16
6174 */
6175 #if _MSC_VER
6176 #ifndef vsnprintf
6177 #define vsnprintf _vsnprintf
6178 #endif
6179 #endif
6180
6181
Coserror(const char * fmt,...)6182 void Coserror(const char *fmt, ...)
6183 {
6184 int retval;
6185 va_list ap;
6186
6187 va_start(ap, fmt);
6188
6189 #ifdef vsnprintf
6190 retval = vsnprintf(coserr.buf, COSERR_len, fmt, ap);
6191 #else
6192 retval = vsprintf(coserr.buf, fmt, ap);
6193 #endif
6194
6195 if (retval == -1)
6196 {
6197 coserr.buf[0] = '\0';
6198 }
6199
6200 va_end(ap);
6201
6202 /* coserror use error number 10 */
6203 if (block_error)
6204 {
6205 *block_error = -5;
6206 }
6207 }
6208 /*--------------------------------------------------------------------------*/
6209 /* SundialsErrHandler: in case of a Sundials error,
6210 * call Coserror() to write it in coserr.buf
6211 *
6212 * The unused parameters are there to square with Sundials' IDA error function, for better genericity.
6213 */
SundialsErrHandler(int error_code,const char * module,const char * function,char * msg,void * user_data)6214 void SundialsErrHandler(int error_code, const char *module, const char *function, char *msg, void *user_data)
6215 {
6216 Coserror("%s: %s", function, msg);
6217 }
6218 /*--------------------------------------------------------------------------*/
6219 /* get_block_error : get the block error
6220 * number
6221 */
get_block_error()6222 int get_block_error()
6223 {
6224 return *block_error;
6225 }
6226 /*--------------------------------------------------------------------------*/
end_scicos_sim()6227 void end_scicos_sim()
6228 {
6229 C2F(coshlt).halt = 2;
6230 return;
6231 }
6232 /*--------------------------------------------------------------------------*/
6233 /* get_pointer_xproperty */
get_pointer_xproperty()6234 int* get_pointer_xproperty()
6235 {
6236 return &xprop[-1 + xptr[C2F(curblk).kfun]];
6237 }
6238 /*--------------------------------------------------------------------------*/
6239 /* get_Npointer_xproperty */
get_npointer_xproperty()6240 int get_npointer_xproperty()
6241 {
6242 return Blocks[C2F(curblk).kfun - 1].nx;
6243 }
6244 /*--------------------------------------------------------------------------*/
6245 /* set_pointer_xproperty */
set_pointer_xproperty(int * pointer)6246 void set_pointer_xproperty(int* pointer)
6247 {
6248 int i;
6249 for (i = 0; i < Blocks[C2F(curblk).kfun - 1].nx; i++)
6250 {
6251 Blocks[C2F(curblk).kfun - 1].xprop[i] = pointer[i];
6252 }
6253 }
6254 /*--------------------------------------------------------------------------*/
6255 /* Jacobian */
Set_Jacobian_flag(int flag)6256 void Set_Jacobian_flag(int flag)
6257 {
6258 Jacobian_Flag = flag;
6259 return;
6260 }
6261 /*--------------------------------------------------------------------------*/
6262 /* Get_Jacobian_ci et Get_Jacobian_cj were called by the C file only produced
6263 by Modelicac v 1.11.2 */
6264 /* double Get_Jacobian_ci(void)
6265 {
6266 return CI;
6267 } */
6268 /*--------------------------------------------------------------------------*/
6269 /* double Get_Jacobian_cj(void)
6270 {
6271 return CJ;
6272 } */
6273 /*--------------------------------------------------------------------------*/
6274 /* Function called by the C file produced by Modelicac 1.7.3 and 1.12.1 */
Get_Jacobian_parameter(void)6275 double Get_Jacobian_parameter(void)
6276 {
6277 return CJJ;
6278 }
6279 /*--------------------------------------------------------------------------*/
Get_Scicos_SQUR(void)6280 double Get_Scicos_SQUR(void)
6281 {
6282 return SQuround;
6283 }
6284 /*--------------------------------------------------------------------------*/
Jacobians(long int Neq,realtype tt,realtype cj,N_Vector yy,N_Vector yp,N_Vector resvec,DlsMat Jacque,void * jdata,N_Vector tempv1,N_Vector tempv2,N_Vector tempv3)6285 static int Jacobians(long int Neq, realtype tt, realtype cj, N_Vector yy,
6286 N_Vector yp, N_Vector resvec, DlsMat Jacque, void *jdata,
6287 N_Vector tempv1, N_Vector tempv2, N_Vector tempv3)
6288 {
6289 double ttx = 0;
6290 double *xc = NULL, *xcdot = NULL, *residual = NULL;
6291 /* char chr;*/
6292 int i = 0, j = 0, n = 0, nx = 0, ni = 0, no = 0, nb = 0, m = 0, flag = 0;
6293 double *RX = NULL, *Fx = NULL, *Fu = NULL, *Gx = NULL, *Gu = NULL, *ERR1 = NULL, *ERR2 = NULL;
6294 double *Hx = NULL, *Hu = NULL, *Kx = NULL, *Ku = NULL, *HuGx = NULL, *FuKx = NULL, *FuKuGx = NULL, *HuGuKx = NULL;
6295 double ysave = 0;
6296 int job = 0;
6297 double **y = NULL;
6298 double **u = NULL;
6299 /* taill1= 3*n+(n+ni)*(n+no)+nx(2*nx+ni+2*m+no)+m*(2*m+no+ni)+2*ni*no*/
6300 double inc = 0., inc_inv = 0., xi = 0., xpi = 0., srur = 0.;
6301 realtype *Jacque_col = NULL;
6302
6303 UserData data;
6304 realtype hh = 0.;
6305 N_Vector ewt;
6306 double *ewt_data = NULL;
6307
6308 *ierr = 0;
6309
6310 data = (UserData) jdata;
6311 ewt = data->ewt;
6312
6313 flag = IDAGetCurrentStep(data->dae_mem, &hh);
6314 if (flag < 0)
6315 {
6316 *ierr = 200 + (-flag);
6317 return (*ierr);
6318 };
6319
6320 flag = IDAGetErrWeights(data->dae_mem, ewt);
6321 if (flag < 0)
6322 {
6323 *ierr = 200 + (-flag);
6324 return (*ierr);
6325 };
6326
6327 ewt_data = NV_DATA_S(ewt);
6328 xc = (double *) N_VGetArrayPointer(yy);
6329 xcdot = (double *) N_VGetArrayPointer(yp);
6330 /*residual=(double *) NV_DATA_S(resvec);*/
6331 ttx = (double)tt;
6332 // CJ=(double)cj; // for fonction Get_Jacobian_cj
6333 CJJ = (double)cj; // returned by Get_Jacobian_parameter
6334
6335 srur = (double) RSqrt(UNIT_ROUNDOFF);
6336
6337 if (AJacobian_block > 0)
6338 {
6339 nx = Blocks[AJacobian_block - 1].nx; /* quant on est là cela signifie que AJacobian_block>0 */
6340 no = Blocks[AJacobian_block - 1].nout;
6341 ni = Blocks[AJacobian_block - 1].nin;
6342 y = (double **)Blocks[AJacobian_block - 1].outptr; /*for compatibility */
6343 u = (double **)Blocks[AJacobian_block - 1].inptr; /*warning pointer of y and u have changed to void ***/
6344 }
6345 else
6346 {
6347 nx = 0;
6348 no = 0;
6349 ni = 0;
6350 }
6351 n = Neq;
6352 nb = nblk;
6353 m = n - nx;
6354
6355 residual = (double *)data->rwork;
6356 ERR1 = residual + n;
6357 ERR2 = ERR1 + n;
6358 RX = ERR2 + n;
6359 Fx = RX + (n + ni) * (n + no); /* car (nx+ni)*(nx+no) peut etre > `a n*n*/
6360 Fu = Fx + nx * nx;
6361 Gx = Fu + nx * ni;
6362 Gu = Gx + no * nx;
6363 Hx = Gu + no * ni;
6364 Hu = Hx + m * m;
6365 Kx = Hu + m * no;
6366 Ku = Kx + ni * m;
6367 HuGx = Ku + ni * no;
6368 FuKx = HuGx + m * nx;
6369 FuKuGx = FuKx + nx * m;
6370 HuGuKx = FuKuGx + nx * nx;
6371 /* HuGuKx+m*m; => m*m=size of HuGuKx */
6372 /* ------------------ Numerical Jacobian--->> Hx,Kx */
6373
6374 /* read residuals;*/
6375 job = 0;
6376 Jdoit(&ttx, xc, xcdot, residual, &job);
6377 if (*ierr < 0)
6378 {
6379 return -1;
6380 }
6381
6382 /* "residual" already contains the current residual,
6383 so the first call to Jdoit can be removed */
6384
6385 for (i = 0; i < m; i++)
6386 for (j = 0; j < ni; j++)
6387 {
6388 Kx[j + i * ni] = u[j][0];
6389 }
6390
6391 for (i = 0; i < m; i++)
6392 {
6393 xi = xc[i];
6394 xpi = xcdot[i];
6395 inc = MAX( srur * MAX( ABS(xi), ABS(hh * xpi)), ONE / ewt_data[i] );
6396 if (hh * xpi < ZERO)
6397 {
6398 inc = -inc;
6399 }
6400 inc = (xi + inc) - xi;
6401
6402 /* if (CI==0) {
6403 inc = MAX( srur * ABS(hh*xpi),ONE );
6404 if (hh*xpi < ZERO) inc = -inc;
6405 inc = (xpi + inc) - xi;
6406 } */
6407 // xc[i] += CI*inc;
6408 // xcdot[i] += CJ*inc;
6409 xc[i] += inc;
6410 xcdot[i] += CJJ * inc;
6411 /*a= Max(abs(H[0]*xcdot[i]),abs(1.0/Ewt[i]));
6412 b= Max(1.0,abs(xc[i]));
6413 del=SQUR[0]*Max(a,b); */
6414 job = 0; /* read residuals */
6415 Jdoit(&ttx, xc, xcdot, ERR2, &job);
6416 if (*ierr < 0)
6417 {
6418 return -1;
6419 }
6420 inc_inv = ONE / inc;
6421 for (j = 0; j < m; j++)
6422 {
6423 Hx[m * i + j] = (ERR2[j] - residual[j]) * inc_inv;
6424 }
6425 for (j = 0; j < ni; j++)
6426 {
6427 Kx[j + i * ni] = (u[j][0] - Kx[j + i * ni]) * inc_inv;
6428 }
6429 xc[i] = xi;
6430 xcdot[i] = xpi;
6431 }
6432 /*----- Numerical Jacobian--->> Hu,Ku */
6433
6434 if (AJacobian_block == 0)
6435 {
6436 for (j = 0; j < m; j++)
6437 {
6438 Jacque_col = DENSE_COL(Jacque, j);
6439 for (i = 0; i < m; i++)
6440 {
6441 Jacque_col[i] = Hx[i + j * m];
6442 }
6443 }
6444 C2F(ierode).iero = *ierr;
6445 return 0;
6446 }
6447 /****------------------***/
6448 job = 0;
6449 Jdoit(&ttx, xc, xcdot, ERR1, &job);
6450 for (i = 0; i < no; i++)
6451 for (j = 0; j < ni; j++)
6452 {
6453 Ku[j + i * ni] = u[j][0];
6454 }
6455
6456 for (i = 0; i < no; i++)
6457 {
6458 ysave = y[i][0];
6459 inc = srur * MAX( ABS(ysave), 1);
6460 inc = (ysave + inc) - ysave;
6461 /*del=SQUR[0]* Max(1.0,abs(y[i][0]));
6462 del=(y[i][0]+del)-y[i][0];*/
6463 y[i][0] += inc;
6464 job = 2; /* applying y[i][0] to the output of imp block*/
6465 Jdoit(&ttx, xc, xcdot, ERR2, &job);
6466 if (*ierr < 0)
6467 {
6468 return -1;
6469 }
6470 inc_inv = ONE / inc;
6471 for (j = 0; j < m; j++)
6472 {
6473 Hu[m * i + j] = (ERR2[j] - ERR1[j]) * inc_inv;
6474 }
6475 for (j = 0; j < ni; j++)
6476 {
6477 Ku[j + i * ni] = (u[j][0] - Ku[j + i * ni]) * inc_inv;
6478 }
6479 y[i][0] = ysave;
6480 }
6481 /*----------------------------------------------*/
6482 job = 1; /* read jacobian through flag=10; */
6483 if (block_error)
6484 {
6485 *block_error = 0;
6486 }
6487 Jdoit(&ttx, xc, xcdot, &Fx[-m], &job);/* Filling up the FX:Fu:Gx:Gu*/
6488 if (block_error && *block_error != 0)
6489 {
6490 sciprint(_("\n error in Jacobian"));
6491 }
6492 /*-------------------------------------------------*/
6493
6494 Multp(Fu, Ku, RX, nx, ni, ni, no);
6495 Multp(RX, Gx, FuKuGx, nx, no, no, nx);
6496
6497 for (j = 0; j < nx; j++)
6498 {
6499 Jacque_col = DENSE_COL(Jacque, j + m);
6500 for (i = 0; i < nx; i++)
6501 {
6502 Jacque_col[i + m] = Fx[i + j * nx] + FuKuGx[i + j * nx];
6503 }
6504 }
6505
6506 Multp(Hu, Gx, HuGx, m, no, no, nx);
6507
6508 for (i = 0; i < nx; i++)
6509 {
6510 Jacque_col = DENSE_COL(Jacque, i + m);
6511 for (j = 0; j < m; j++)
6512 {
6513 Jacque_col[j] = HuGx[j + i * m];
6514 }
6515 }
6516
6517 Multp(Fu, Kx, FuKx, nx, ni, ni, m);
6518
6519 for (i = 0; i < m; i++)
6520 {
6521 Jacque_col = DENSE_COL(Jacque, i);
6522 for (j = 0; j < nx; j++)
6523 {
6524 Jacque_col[j + m] = FuKx[j + i * nx];
6525 }
6526 }
6527
6528
6529 Multp(Hu, Gu, RX, m, no, no, ni);
6530 Multp(RX, Kx, HuGuKx, m, ni, ni, m);
6531
6532 for (j = 0; j < m; j++)
6533 {
6534 Jacque_col = DENSE_COL(Jacque, j);
6535 for (i = 0; i < m; i++)
6536 {
6537 Jacque_col[i] = Hx[i + j * m] + HuGuKx[i + j * m];
6538 }
6539 }
6540
6541 /* chr='Z'; printf("\n t=%g",ttx); DISP(Z,n,n,chr);*/
6542 C2F(ierode).iero = *ierr;
6543 return 0;
6544
6545 }
6546 /*--------------------------------------------------------------------------*/
Multp(double * A,double * B,double * R,int ra,int rb,int ca,int cb)6547 static void Multp(double *A, double *B, double *R, int ra, int rb, int ca, int cb)
6548 {
6549 int i = 0, j = 0, k = 0;
6550 /*if (ca!=rb) sciprint(_("\n Error in matrix multiplication"));*/
6551 for (i = 0; i < ra; i++)
6552 for (j = 0; j < cb; j++)
6553 {
6554 R[i + ra * j] = 0.0;
6555 for (k = 0; k < ca; k++)
6556 {
6557 R[i + ra * j] += A[i + k * ra] * B[k + j * rb];
6558 }
6559 }
6560 return;
6561 }
6562 /*--------------------------------------------------------------------------*/
read_xml_initial_states(int nvar,const char * xmlfile,char ** ids,double * svars)6563 int read_xml_initial_states(int nvar, const char * xmlfile, char **ids, double *svars)
6564 {
6565 ezxml_t model, elements;
6566 int result = 0, i = 0;
6567 double vr = 0.;
6568
6569 if (nvar == 0)
6570 {
6571 return 0;
6572 }
6573 result = 0;
6574 for (i = 0; i < nvar; i++)
6575 {
6576 if (strcmp(ids[i], "") != 0)
6577 {
6578 result = 1;
6579 break;
6580 }
6581 }
6582 if (result == 0)
6583 {
6584 return 0;
6585 }
6586
6587 model = ezxml_parse_file(xmlfile);
6588
6589 if (model == NULL)
6590 {
6591 sciprint(_("Error: Cannot find file '%s'.\n"), xmlfile);
6592 return -1;/* file does not exist*/
6593 }
6594
6595 elements = ezxml_child(model, "elements");
6596 for (i = 0; i < nvar; i++)
6597 {
6598 vr = 0.0;
6599 result = read_id(&elements, ids[i], &vr);
6600 if (result == 1)
6601 {
6602 svars[i] = vr;
6603 }
6604 }
6605 ezxml_free(model);
6606 return 0;
6607 }
6608 /*--------------------------------------------------------------------------*/
read_id(ezxml_t * elements,char * id,double * value)6609 static int read_id(ezxml_t *elements, char *id, double *value)
6610 {
6611 char V1[100], V2[100];
6612 int ok = 0, i = 0, ln = 0;
6613
6614 if (strcmp(id, "") == 0)
6615 {
6616 return 0;
6617 }
6618 ok = search_in_child(elements, id, V1);
6619 if (ok == 0 )
6620 {
6621 /*sciprint(_("Cannot find: %s=%s \n"),id,V1); */
6622 return 0;
6623 }
6624 else
6625 {
6626 if (Convert_number(V1, value) != 0)
6627 {
6628 ln = (int)(strlen(V1));
6629 if (ln > 2)
6630 {
6631 for (i = 1; i <= ln - 2; i++)
6632 {
6633 V2[i - 1] = V1[i];
6634 }
6635 V2[ln - 2] = '\0';
6636 ok = read_id(elements, V2, value);
6637 return ok;
6638 }
6639 else
6640 {
6641 return 0;
6642 }
6643 }
6644 else
6645 {
6646 /* printf("\n ---->>>%s= %g",V1,*value);*/
6647 return 1;
6648 }
6649 }
6650 }
6651 /*--------------------------------------------------------------------------*/
Convert_number(char * s,double * out)6652 int Convert_number(char *s, double *out)
6653 {
6654 char *endp = NULL;
6655 double d = 0.;
6656 long int l = 0;
6657 d = strtod(s, &endp);
6658 if (s != endp && *endp == '\0')
6659 {
6660 /* printf(" It's a float with value %g ", d); */
6661 *out = d;
6662 return 0;
6663 }
6664 else
6665 {
6666 l = strtol(s, &endp, 0);
6667 if (s != endp && *endp == '\0')
6668 {
6669 /*printf(" It's an int with value %ld ", 1); */
6670 *out = (double)l;
6671 return 0;
6672 }
6673 else
6674 {
6675 /*printf(" string "); */
6676 return -1;
6677 }
6678 }
6679 }
6680 /*--------------------------------------------------------------------------*/
write_xml_states(int nvar,const char * xmlfile,char ** ids,double * x)6681 int write_xml_states(int nvar, const char * xmlfile, char **ids, double *x)
6682 {
6683 ezxml_t model, elements;
6684 int result = 0, i = 0, err = 0;
6685 FILE *fd = NULL;
6686 char *s = NULL;
6687 char **xv = NULL;
6688
6689 if (nvar == 0)
6690 {
6691 return 0;
6692 }
6693 result = 0;
6694 for (i = 0; i < nvar; i++)
6695 {
6696 if (strcmp(ids[i], "") != 0)
6697 {
6698 result = 1;
6699 break;
6700 }
6701 }
6702 if (result == 0)
6703 {
6704 return 0;
6705 }
6706
6707 xv = MALLOC(nvar * sizeof(char*));
6708 for (i = 0; i < nvar; i++)
6709 {
6710 xv[i] = MALLOC(nvar * 100 * sizeof(char));
6711 sprintf(xv[i], "%g", x[i]);
6712 }
6713
6714 model = ezxml_parse_file(xmlfile);
6715 if (model == NULL)
6716 {
6717 sciprint(_("Error: Cannot find file '%s'.\n"), xmlfile);
6718 err = -1;
6719 goto err_free_xv;
6720 }
6721
6722 elements = ezxml_child(model, "elements");
6723
6724 for (i = 0; i < nvar; i++)
6725 {
6726 if (strcmp(ids[i], "") == 0)
6727 {
6728 continue;
6729 }
6730 result = write_in_child(&elements, ids[i], xv[i]);
6731 if (result == 0 )
6732 {
6733 /* sciprint(_("cannot find %s in '%s' \n"),ids[i],xmlfile); */
6734 /* err= -1;*/ /* Variable does not exist*/
6735 }
6736 }
6737
6738 s = ezxml_toxml(model);
6739 ezxml_free(model);
6740
6741
6742 wcfopen(fd, (char*)xmlfile, "wb");
6743 if (fd == NULL)
6744 {
6745 err = -3;/* cannot write to file*/
6746 goto err_free_s;
6747 }
6748
6749 fputs (s, fd);
6750 fclose(fd);
6751
6752 err_free_s:
6753 free(s);
6754 err_free_xv:
6755 for (i = 0; i < nvar; i++)
6756 {
6757 FREE(xv[i]);
6758 }
6759 FREE(xv);
6760 return err;
6761 }
6762 /*--------------------------------------------------------------------------*/
C2F(fx)6763 int C2F(fx)(double *x, double *residual) /* used for homotopy*/
6764 {
6765 double *xdot = NULL, t = 0;
6766 xdot = x + *neq;
6767 t = 0;
6768 *ierr = 0;
6769 C2F(ierode).iero = 0;
6770 odoit(&t, x, xdot, residual);
6771 C2F(ierode).iero = *ierr;
6772 return (*ierr);
6773 }
6774 /*--------------------------------------------------------------------------*/
rho_(double * a,double * L,double * x,double * rho,double * rpar,int * ipar)6775 int rho_(double *a, double *L, double *x, double *rho, double *rpar, int *ipar) /* used for homotopy*/
6776 {
6777 int i = 0, N = 0;
6778 N = *neq;
6779
6780 fx_(x, rho);
6781 for (i = 0; i < N; i++)
6782 {
6783 rho[i] += (-1 + *L) * a[i];
6784 }
6785 return 0;
6786 }
6787 /*--------------------------------------------------------------------------*/
rhojac_(double * a,double * lambda,double * x,double * jac,int * col,double * rpar,int * ipar)6788 int rhojac_(double *a, double *lambda, double *x, double *jac, int *col, double *rpar, int *ipar) /* used for homotopy*/
6789 {
6790 /* MATRIX [d_RHO/d_LAMBDA, d_RHO/d_X_col] */
6791 int j = 0, N = 0;
6792 double *work = NULL;
6793 double inc = 0., inc_inv = 0., xi = 0., srur = 0.;
6794 N = *neq;
6795 if (*col == 1)
6796 {
6797 for (j = 0; j < N; j++)
6798 {
6799 jac[j] = a[j];
6800 }
6801 }
6802 else
6803 {
6804 if ((work = (double *) MALLOC(N * sizeof(double))) == NULL)
6805 {
6806 *ierr = 10000;
6807 return *ierr;
6808 }
6809 rho_(a, lambda, x, work, rpar, ipar);
6810 srur = 1e-10;
6811 xi = x[*col - 2];
6812 inc = srur * Max(fabs(xi), 1);
6813 inc = (xi + inc) - xi;
6814 x[*col - 2] += inc;
6815
6816 rho_(a, lambda, x, jac, rpar, ipar);
6817 inc_inv = 1.0 / inc;
6818
6819 for (j = 0; j < N; j++)
6820 {
6821 jac[j] = (jac[j] - work[j]) * inc_inv;
6822 }
6823
6824 x[*col - 2] = xi;
6825 FREE(work);
6826 }
6827 return 0;
6828 }
6829 /*--------------------------------------------------------------------------*/
C2F(hfjac)6830 int C2F(hfjac)(double *x, double *jac, int *col)
6831 {
6832 int N = 0, j = 0;
6833 double *work = NULL;
6834 double *xdot = NULL;
6835 double inc = 0., inc_inv = 0., xi = 0., srur = 0.;
6836
6837 N = *neq;
6838 if ((work = (double *) MALLOC(N * sizeof(double))) == NULL)
6839 {
6840 *ierr = 10000;
6841 return *ierr;
6842 }
6843 srur = (double) RSqrt(UNIT_ROUNDOFF);
6844
6845 fx_(x, work);
6846
6847 xi = x[*col - 1];
6848 inc = srur * MAX (ABS(xi), 1);
6849 inc = (xi + inc) - xi;
6850 x[*col - 1] += inc;
6851 xdot = x + N;
6852
6853 fx_(x, jac);
6854 if (*ierr < 0)
6855 {
6856 FREE(work);
6857 return *ierr;
6858 }
6859
6860 inc_inv = ONE / inc;
6861
6862 for (j = 0; j < N; j++)
6863 {
6864 jac[j] = (jac[j] - work[j]) * inc_inv;
6865 }
6866
6867 x[*col - 1] = xi;
6868
6869 FREE(work);
6870 return 0;
6871 }
6872 /*--------------------------------------------------------------------------*/
simblkKinsol(N_Vector yy,N_Vector resval,void * rdata)6873 int simblkKinsol(N_Vector yy, N_Vector resval, void *rdata)
6874 {
6875 double t = 0., *xc = NULL, *xcdot = NULL, *residual = NULL;
6876 UserData data;
6877 int jj = 0, nantest = 0, N = 0;
6878 N = *neq;
6879
6880 t = 0;
6881 xc = (double *) NV_DATA_S(yy);
6882 residual = (double *) NV_DATA_S(resval);
6883 data = (UserData) rdata;
6884 xcdot = xc;
6885 if (phase == 1) if ( ng > 0 && nmod > 0 )
6886 {
6887 zdoit(&t, xc, xcdot, g);
6888 }
6889
6890 *ierr = 0;
6891 C2F(ierode).iero = 0;
6892 odoit(&t, xc, xcdot, residual);
6893
6894 if (*ierr == 0)
6895 {
6896 nantest = 0; /* NaN checking */
6897 for (jj = 0; jj < N; jj++)
6898 {
6899 if (residual[jj] - residual[jj] != 0)
6900 {
6901 Sciwarning(_("\nWarning: The initialization system #%d returns a NaN/Inf"), jj);
6902 nantest = 1;
6903 break;
6904 }
6905 }
6906 if (nantest == 1)
6907 {
6908 return 258; /* recoverable error; */
6909 }
6910 }
6911 C2F(ierode).iero = *ierr;
6912
6913 return (abs(*ierr)); /* ierr>0 recoverable error; ierr>0 unrecoverable error; ierr=0: ok*/
6914 }
6915 /*--------------------------------------------------------------------------*/
CallKinsol(double * told)6916 static int CallKinsol(double *told)
6917 {
6918 //** used for the [stop] button
6919 char* CommandToUnstack;
6920 static int CommandLength = 0;
6921 static int SeqSync = 0;
6922 static int one = 1;
6923
6924 N_Vector y = NULL, yscale = NULL, fscale = NULL;
6925 double *fsdata = NULL, *ysdata = NULL;
6926 int N = 0, strategy = 0, i = 0, j = 0, k = 0, status = 0;
6927 /* int mxiter, msbset, msbsetsub, etachoice, mxnbcf; */
6928 /* double eta, egamma, ealpha, mxnewtstep, relfunc, fnormtol, scsteptol; */
6929 /* booleantype noInitSetup, noMinEps; */
6930 void *kin_mem = NULL;
6931 realtype reltol = 0., abstol = 0.;
6932 int *Mode_save = NULL;
6933 int Mode_change = 0;
6934 static int PH = 0;
6935 int N_iters = 0;
6936 double ratio = 0.;
6937
6938 N = *neq;
6939 if (N <= 0)
6940 {
6941 return 0;
6942 }
6943
6944 reltol = (realtype) rtol;
6945 abstol = (realtype) Atol;
6946
6947 Mode_save = NULL;
6948 if (nmod > 0)
6949 {
6950 if ((Mode_save = MALLOC(sizeof(int) * nmod)) == NULL )
6951 {
6952 *ierr = 10000;
6953 return -1;
6954 }
6955 }
6956
6957 y = N_VNewEmpty_Serial(N);
6958 if (y == NULL)
6959 {
6960 FREE(Mode_save);
6961 return -1;
6962 }
6963 yscale = N_VNew_Serial(N);
6964 if (yscale == NULL)
6965 {
6966 FREE(Mode_save);
6967 N_VDestroy_Serial(y);
6968 return -1;
6969 }
6970 fscale = N_VNew_Serial(N);
6971 if (fscale == NULL)
6972 {
6973 FREE(Mode_save);
6974 N_VDestroy_Serial(y);
6975 N_VDestroy_Serial(yscale);
6976 return -1;
6977 }
6978 ysdata = NV_DATA_S(yscale);
6979 fsdata = NV_DATA_S(fscale);
6980
6981 NV_DATA_S(y) = x;
6982 kin_mem = KINCreate();
6983 if (kin_mem == NULL)
6984 {
6985 FREE(Mode_save);
6986 N_VDestroy_Serial(y);
6987 N_VDestroy_Serial(yscale);
6988 N_VDestroy_Serial(fscale);
6989 return -1;
6990 }
6991
6992 status = KINInit(kin_mem, simblkKinsol, y);
6993 strategy = KIN_NONE; /*without LineSearch */
6994 status = KINDense(kin_mem, N);
6995
6996 status = KINSetNumMaxIters(kin_mem, 2000); /* MaxNumIter=200->2000 */
6997 status = KINSetRelErrFunc(kin_mem, reltol); /* FuncRelErr=eps->RTOL */
6998 status = KINSetMaxSetupCalls(kin_mem, 1); /* MaxNumSetups=10->1=="Exact Newton" */
6999 status = KINSetMaxSubSetupCalls(kin_mem, 1); /* MaxNumSubSetups=5->1 */
7000 /* status = KINSetNoInitSetup(kin_mem,noInitSetup); // InitialSetup=true */
7001 /* status = KINSetNoMinEps(kin_mem,noMinEps); // MinBoundEps=true */
7002 /* status = KINSetMaxBetaFails(kin_mem,mxnbcf); // MaxNumBetaFails=10 */
7003 /* status = KINSetEtaForm(kin_mem,etachoice); // EtaForm=Type1 */
7004 /* status = KINSetEtaConstValue(kin_mem,eta);*/ // Eta=0.1 */
7005 /* status = KINSetEtaParams(kin_mem,egamma,ealpha); // EtaGamma=0.9 EtaAlpha=2.0 */
7006 /* status = KINSetMaxNewtonStep(kin_mem,mxnewtstep); // MaxNewtonStep=0.0 */
7007 /* status = KINSetFuncNormTol(kin_mem,fnormtol); // FuncNormTol=eps^(1/3) */
7008 /* status = KINSetScaledStepTol(kin_mem,scsteptol); // ScaledStepTol={eps^(2/3) */
7009 for ( j = 0; j < N; j++)
7010 {
7011 ysdata[j] = 1;
7012 fsdata[j] = 1;
7013 }
7014 /*========================================================*/
7015 if (PH == 2)
7016 {
7017 PH = 1;
7018 }
7019 else
7020 {
7021 PH = 2; /* remind that PH is a static variable*/
7022 }
7023
7024 status = -1;
7025 N_iters = 10 + nmod * 3;
7026 for (k = 0; k <= N_iters; k++) /* loop for mode fixin*/
7027 {
7028 phase = PH;
7029 /*------------KINSOL calls-----------*/
7030 for (i = 0; i < 10; i++)
7031 {
7032 simblkKinsol(y, fscale, NULL);
7033
7034 for (j = 0; j < N; j++)
7035 if (fsdata[j] - fsdata[j] != 0)
7036 {
7037 sciprint(_("\nWarning: The residual function #%d returns a NaN/Inf"), j);
7038 sciprint(_("\n The residual function returns NAN/Inf. \n Please verify your model:\n some functions might be called with illegal inputs."));
7039 freekinsol;
7040 *ierr = 400 - status;
7041 C2F(ierode).iero = *ierr;
7042 return -1;
7043 }
7044 ratio = 0.3;
7045 for ( j = 0; j < N; j++)
7046 {
7047 if (x[j] == 0)
7048 {
7049 ysdata[j] += 1 * ratio;
7050 }
7051 else
7052 {
7053 ysdata[j] += ratio / fabs(x[j]);
7054 }
7055 if (fsdata[j] == 0)
7056 {
7057 fsdata[j] = 1;
7058 }
7059 else
7060 {
7061 fsdata[j] = 1 / fabs(fsdata[j]);
7062 }
7063 ysdata[j] /= ratio + 1;
7064 }
7065 status = KINSol(kin_mem, y, strategy, yscale, fscale);/* Calling the Newton Solver */
7066 if (status >= 0)
7067 {
7068 break;
7069 }
7070 /* Serge Steer 29/06/2009 */
7071 while (ismenu()) //** if the user has done something, do the actions
7072 {
7073 int ierr2 = 0;
7074 int iUnused;
7075 GetCommand(&CommandToUnstack, &SeqSync, &iUnused, NONE); //** get to the action
7076 CommandLength = (int)strlen(CommandToUnstack);
7077 //syncexec(CommandToUnstack, &CommandLength, &ierr2, &one, CommandLength); //** execute it
7078 FREE(CommandToUnstack);
7079 }
7080
7081 if (C2F(coshlt).halt != 0)
7082 {
7083 C2F(coshlt).halt = 0;
7084 freekinsol;
7085 return 0;
7086 }
7087 }
7088 /*---------end of KINSOL calls-----------*/
7089 if (PH == 2 )
7090 {
7091 for (j = 0; j < nmod; ++j)
7092 {
7093 Mode_save[j] = mod[j];
7094 }
7095
7096 if (ng > 0 && nmod > 0)
7097 {
7098 phase = 1; // updating the modes
7099 zdoit(told, x, xd, g);
7100 if (*ierr != 0)
7101 {
7102 freekinsol;
7103 C2F(ierode).iero = *ierr;
7104 return -1;
7105 }
7106 }
7107
7108 Mode_change = 0;
7109 for (j = 0; j < nmod; ++j)
7110 {
7111 if (Mode_save[j] != mod[j])
7112 {
7113 Mode_change = 1;
7114 break;
7115 }
7116 }
7117
7118 if (Mode_change == 0 && status >= 0 )
7119 {
7120 break; /*Successful termination*/
7121 }
7122
7123 }
7124 else
7125 {
7126 /* calling with phase=1*/
7127 if (status >= 0)
7128 {
7129 break;
7130 }
7131 }
7132
7133 } /* end of the loop for mode fixing*/
7134
7135 if (status < 0 )
7136 {
7137 *ierr = 400 - status;
7138 C2F(ierode).iero = *ierr;
7139 }
7140 freekinsol;
7141 return status;
7142 } /* CallKinSol_ */
7143 /*--------------------------------------------------------------------------*/
7144
7145