solvers/ida/idaboundary.c

#include "ida.h"
#include "idalinear.h"
#include "idaanalyse.h"
#include "idatypes.h"
#include "idaprec.h"
#include "idacalc.h"
#include "idaio.h"
#include "idaboundary.h"
#include <stdio.h>

#include <ascend/general/platform.h>
#include <ascend/general/list.h>
#include <ascend/general/panic.h>

#include <ascend/solver/solver.h>

#include <ascend/system/slv_client.h>
#include <ascend/system/cond_config.h>
#include <ascend/system/discrete.h>
#include <ascend/system/slv_types.h>
#include <ascend/system/slv_common.h>
#include <ascend/system/logrel.h>
#include <ascend/system/rel.h>
#include <ascend/system/discrete.h>
#include <ascend/system/var.h>
#include <ascend/system/block.h>
#include <ascend/system/bndman.h>

#define IDA_BND_DEBUG

/**
 * Check to see if any of the system discrete variables have changed.
 *
 * @return 1 if any of the values have changed (does not necessarily mean
 *           the system needs to be reconfigured)
 */
int some_dis_vars_changed(slv_system_t sys) {
    struct dis_discrete **dvlist, *cur_dis;
    int numDVs, i;
#ifdef IDA_BND_DEBUG
    char *dis_name;
#endif

    dvlist = slv_get_solvers_dvar_list(sys);
    numDVs = slv_get_num_solvers_dvars(sys);

    for (i = 0; i < numDVs; i++) {
        cur_dis = dvlist[i];
        if((dis_kind(cur_dis) == e_dis_boolean_t)
            && (dis_inwhen(cur_dis) || dis_inevent(cur_dis))
        ){
            if(dis_value(cur_dis) != dis_previous_value(cur_dis)){
#ifdef IDA_BND_DEBUG
                dis_name = dis_make_name(sys, cur_dis);
                CONSOLE_DEBUG("Boolean %s (i=%d) has changed (current=%d, prev=%d)", dis_name,
                        i, dis_value(cur_dis), dis_previous_value(cur_dis));
                ASC_FREE(dis_name);
#endif
                return 1;
            }
        }
    }
    CONSOLE_DEBUG("No boundary vars have changed");
    return 0;

}

static void ida_write_values(IntegratorSystem *integ) {
    struct var_variable **vl;
    struct dis_discrete **dvl;
    int32 c;
    vl = slv_get_solvers_var_list(integ->system);
    dvl = slv_get_solvers_dvar_list(integ->system);
    for (c = 0; vl[c] != NULL; c++)
        CONSOLE_DEBUG("Value of %s is %f", var_make_name(integ->system,vl[c]),var_value(vl[c]));
    for (c = 0; dvl[c] != NULL; c++)
        CONSOLE_DEBUG("Value of %s is %d", dis_make_name(integ->system,dvl[c]),dis_value(dvl[c]));
}

int ida_setup_lrslv(IntegratorSystem *integ, int qrslv_ind, int lrslv_ind) {
    CONSOLE_DEBUG("Running logical solver...");
    ida_log_solve(integ, lrslv_ind);
    if(some_dis_vars_changed(integ->system)){
        CONSOLE_DEBUG("Some discrete vars changed; reanalysing");
        return ida_bnd_reanalyse_cont(integ);
    }
    return 0;
}

int ida_bnd_reanalyse(IntegratorSystem *integ){

    if (integ->y_id != NULL) {
        ASC_FREE(integ->y_id);
        integ->y_id = NULL;
    }

    if (integ->obs_id != NULL){
        ASC_FREE(integ->obs_id);
        integ->obs_id = NULL;
    }
    if (integ->y != NULL) {
        ASC_FREE(integ->y);
        integ->y = NULL;
    }
    if (integ->ydot != NULL) {
        ASC_FREE(integ->ydot);
        integ->ydot = NULL;
    }
    if (integ->obs != NULL) {
        ASC_FREE(integ->obs);
        integ->obs = NULL;
    }

    integ->n_y = 0;


    return reanalyze_solver_lists(integ->system);
}

int ida_bnd_reanalyse_cont(IntegratorSystem *integ){
    CONSOLE_DEBUG("Clearing memory from previous calls...");
    if (integ->y_id != NULL) {
        ASC_FREE(integ->y_id);
        integ->y_id = NULL;
    }

    if (integ->obs_id != NULL){
        ASC_FREE(integ->obs_id);
        integ->obs_id = NULL;
    }
    if (integ->y != NULL) {
        ASC_FREE(integ->y);
        integ->y = NULL;
    }
    if (integ->ydot != NULL) {
        ASC_FREE(integ->ydot);
        integ->ydot = NULL;
    }
    if (integ->obs != NULL) {
        ASC_FREE(integ->obs);
        integ->obs = NULL;
    }

    integ->n_y = 0;

    return integrator_ida_analyse(integ);
}

int ida_bnd_update_relist(IntegratorSystem *integ){
    IntegratorIdaData *enginedata;
    struct rel_relation **rels;
    int i,j,n_solverrels,n_active_rels;

    enginedata = integrator_ida_enginedata(integ);

    n_solverrels = slv_get_num_solvers_rels(integ->system);
    n_active_rels = slv_count_solvers_rels(integ->system, &integrator_ida_rel);
    rels = slv_get_solvers_rel_list(integ->system);

    if(enginedata->rellist != NULL){
        ASC_FREE(enginedata->rellist);
        enginedata->rellist = NULL;
        enginedata->rellist = ASC_NEW_ARRAY(struct rel_relation *, n_active_rels);
    }

    j = 0;
    for (i = 0; i < n_solverrels; ++i) {
        if (rel_apply_filter(rels[i], &integrator_ida_rel)) {
#ifdef IDA_BND_DEBUG
            char *relname;
            relname = rel_make_name(integ->system, rels[i]);
            CONSOLE_DEBUG("rel '%s': 0x%x", relname, rel_flags(rels[i]));
            ASC_FREE(relname);
#endif
            enginedata->rellist[j++] = rels[i];
        }
    }
    asc_assert(j == n_active_rels);
    enginedata->nrels = n_active_rels;

    if (enginedata->nrels != integ->n_y) {
        ERROR_REPORTER_HERE(ASC_USER_ERROR
                ,"Integration problem is not square (%d active rels, %d vars)"
                ,n_active_rels, integ->n_y
        );
        return 1; /* failure */
    }

    return 0;
}

N_Vector ida_bnd_new_zero_NV(long int vec_length){
    int i;
    N_Vector nv = N_VNew_Serial(vec_length);
    for(i= 0; i< vec_length; i++) {
        NV_Ith_S(nv,i) = 0.0;
    }

    return nv;
}


void ida_bnd_update_IC(IntegratorSystem *integ, realtype t0, N_Vector y0, N_Vector yp0) {
    /* First destroy since n_y may have changed */
    N_VDestroy_Serial(y0);
    N_VDestroy_Serial(yp0);
    /* retrieve new initial values from the system */
    t0 = integrator_get_t(integ);
    y0 = ida_bnd_new_zero_NV(integ->n_y);
    integrator_get_y(integ, NV_DATA_S(y0));

    yp0 = ida_bnd_new_zero_NV(integ->n_y);
    integrator_get_ydot(integ, NV_DATA_S(yp0));

#ifdef IDA_BND_DEBUG
    CONSOLE_DEBUG("BEFORE IC SOLVING:");
    CONSOLE_DEBUG("TIME: %f", t0);
    CONSOLE_DEBUG("Y");
    N_VPrint_Serial(y0);
    CONSOLE_DEBUG("Yp");
    N_VPrint_Serial(yp0);
    CONSOLE_DEBUG("Press any to continue...");
    getchar();
#endif

}

int ida_log_solve(IntegratorSystem *integ, int lrslv_ind) {
    slv_parameters_t parameters;
    int num_params;
    char *pname;
    slv_status_t status;
    int i;
    if (slv_select_solver(integ->system, lrslv_ind) == -1) {
        ERROR_REPORTER_HERE(ASC_PROG_ERR,"Error attempting to load LRSlv");
    }
#ifdef IDA_BND_DEBUG
    CONSOLE_DEBUG("Solver selected is '%s'"
        ,slv_solver_name(slv_get_selected_solver(integ->system))
    );
#endif

    /* Flip LRSlv into ida mode */
    slv_get_parameters(integ->system, &parameters);
    num_params = parameters.num_parms;
    for (i = 0; i<num_params; i++) {
        pname = parameters.parms[i].name;
        if(strcmp(pname, "withida") == 0) {
            parameters.parms[i].info.b.value = 1;
        }
    }
    /* solve the logical relations in the model, if possible */
    slv_presolve(integ->system);
    slv_solve(integ->system);

    /* Check for convergence */
    slv_get_status(integ->system, &status);
    if (!status.converged) {
        ERROR_REPORTER_HERE(ASC_PROG_ERR,"Non-convergence in logical solver.");
        return 0;
#ifdef IDA_BND_DEBUG
    }else{
        CONSOLE_DEBUG("Converged");
#endif
    }
    return 1;
}

/*
 * Uses LRSlv to check if any of the logical conditions in the model need
 * updating after a boundary crossing. If so, the model is reconfigured and
 * new initial conditions are determined at the point of the crossing.
 *
 * @return 1 if the crossing causes a change in the system
 */

int ida_cross_boundary(IntegratorSystem *integ, int *rootsfound,
        int *bnd_cond_states, int qrslv_ind, int lrslv_ind) {

    IntegratorIdaData *enginedata;
    slv_status_t status;

    struct bnd_boundary *bnd = NULL;
    int i, num_bnds, num_dvars;

    struct dis_discrete **dvl;
    int32 c, *prevals = NULL;

    double *bnd_prev_eval;

    CONSOLE_DEBUG("Crossing boundary...");

    integrator_output_write(integ);
    integrator_output_write_obs(integ);
    integrator_output_write_event(integ);
    /*^^^ FIXME should we write the above event? It may not have crossed in correct direction...? */

    /* Flag the crossed boundary and update bnd_cond_states */
    enginedata = integ->enginedata;
    num_bnds = enginedata->nbnds;
    bnd_prev_eval = ASC_NEW_ARRAY(double,num_bnds);
    for(i = 0; i < num_bnds; i++) {
        /* get the value of the boundary condition before crossing */
        bnd_prev_eval[i] = bndman_real_eval(enginedata->bndlist[i]);
        if(rootsfound[i]){
                    /* reminder: 'rootsfound[i] == 1 for UP or -1 for DOWN. */
            /* this boundary was one of the boundaries triggered */
            bnd = enginedata->bndlist[i];
#ifdef IDA_BND_DEBUG
            char *name = bnd_make_name(integ->system,enginedata->bndlist[i]);
            CONSOLE_DEBUG("Boundary '%s': ida_incr=%d, dirn=%d,ida_value=%d,ida_first_cross=%d,bnd_cond_states[i]=%d"
                ,name,bnd_ida_incr(bnd),rootsfound[i],bnd_ida_value(bnd)
                ,bnd_ida_first_cross(bnd)!=0,bnd_cond_states[i]
            );
#endif
            bnd_set_ida_crossed(bnd, 1);
            if(bnd_ida_first_cross(bnd) /* not crossed before */
                || !((rootsfound[i] == 1 && bnd_ida_incr(bnd)) /* not crossing upwards twice in a row */
                || (rootsfound[i] == -1 && !bnd_ida_incr(bnd))) /* not crossing downwards twice in a row */
            ){
                if(bnd_cond_states[i] == 0){
                    bnd_set_ida_value(bnd, 1);
                    bnd_cond_states[i] = 1;
                }else{
                    bnd_set_ida_value(bnd, 0);
                    bnd_cond_states[i] = 0;
                }
#ifdef IDA_BND_DEBUG
                CONSOLE_DEBUG("Set boundary '%s' to %d (single cross)",name,bnd_ida_value(bnd)!=0);
#endif
            }else{
                /* Boundary crossed twice in one direction. Very unlikey! */

                /* The aim of the following two lines is to set the value of 
                    the boolean variable to such a value as if the boundary 
                    was crossed in the opposite direction before */
                CONSOLE_DEBUG("DOUBLE CROSS");
                if(!prevals){
                    num_dvars = slv_get_num_solvers_dvars(integ->system);
                    dvl = slv_get_solvers_dvar_list(integ->system);
                    prevals = ASC_NEW_ARRAY(int32,num_dvars);
                    for(c = 0; dvl[c] != NULL; c++)
                        prevals[c] = dis_value(dvl[c]);
                }
                bnd_set_ida_value(bnd, !bnd_cond_states[i]);
                if(!ida_log_solve(integ,lrslv_ind)){
                    CONSOLE_DEBUG("Error in logic solve in double-cross");
                    return -1;
                }
                bnd_set_ida_value(bnd,bnd_cond_states[i]);
#ifdef IDA_BND_DEBUG
                CONSOLE_DEBUG("After double-cross, set boundary '%s' to %d",name,bnd_ida_value(bnd)?1:0);
#endif
            }
            /* flag this boundary as having previously been crossed */
            bnd_set_ida_first_cross(bnd,0);
            /* store this recent crossing-direction for future reference */
            bnd_set_ida_incr(bnd,(rootsfound[i] > 0));
#ifdef IDA_BND_DEBUG
            ASC_FREE(name);
#endif
        }
    }
    if(!ida_log_solve(integ,lrslv_ind)) return -1;

    /* If there was a double crossing, because of ida_log_solve the previous values
    of discrete variables may be equal to their current values, which would mean that
    the discrete vars haven't changed their values, but actually they did. So here we
    restore the previous values of those variables, which are not connected with the
    boundary which was double-crossed. */
    if(prevals){
        for(c = 0; dvl[c] != NULL; c++)
            if(!(dis_value(dvl[c]) == prevals[c] && dis_value(dvl[c]) != dis_previous_value(dvl[c])))
                dis_set_previous_value(dvl[c],prevals[c]);
        ASC_FREE(prevals);
    }

    integrator_output_write(integ);
    integrator_output_write_obs(integ);
    integrator_output_write_event(integ);
    if(!some_dis_vars_changed(integ->system)){
        CONSOLE_DEBUG("Crossed boundary but no effect on system");
        /* Boundary crossing that has no effect on system */
        ASC_FREE(bnd_prev_eval);

        /* Reset the boundary flag */
        for(i = 0; i < num_bnds; i++)
            bnd_set_ida_crossed(enginedata->bndlist[i], 0);
        return 0;
    }
    /* update the main system if required */
    int events_triggered = 1;
    if(slv_get_num_solvers_events(integ->system)!=0) {
        while(some_dis_vars_changed(integ->system) && events_triggered)  {
            if(ida_bnd_reanalyse(integ)) {
                /* select QRSlv solver, and solve the system */
                if(slv_select_solver(integ->system, qrslv_ind) == -1) {
                    ERROR_REPORTER_HERE(ASC_PROG_ERR,"Error attempting to load QRSlv");
                }
#ifdef IDA_BND_DEBUG
                {
                    struct var_variable **vl = slv_get_solvers_var_list(integ->system);
                    int i, n=slv_get_num_solvers_vars(integ->system), first=1;
                    CONSOLE_DEBUG("In boundary problem: variables (active, incident, free):");
                    for(i=0;i<n;++i){
                        if(var_incident(vl[i])&&!var_fixed(vl[i])&&var_active(vl[i])){
                            char *name = var_make_name(integ->system,vl[i]);
                            fprintf(stderr,"%s%s",(first?"\t":", "),name);
                            first=0; ASC_FREE(name);
                        }
                    }
                    fprintf(stderr,"\n");
                }
                {
                    struct rel_relation **rl = slv_get_solvers_rel_list(integ->system);
                    int i, n=slv_get_num_solvers_rels(integ->system), first=1;
                    CONSOLE_DEBUG("...relations (equality, included, active):");
                    for(i=0;i<n;++i){
                        if(rel_equality(rl[i])&&rel_active(rl[i])&&rel_included(rl[i])){
                            char *name = rel_make_name(integ->system,rl[i]);
                            fprintf(stderr,"%s%s",(first?"\t":", "),name);
                            first=0; ASC_FREE(name);
                        }
                    }
                    fprintf(stderr,"\n");
                }
#endif
                slv_presolve(integ->system);
                slv_solve(integ->system);

                slv_get_status(integ->system, &status);
                if (!status.converged) {
                    ERROR_REPORTER_HERE(ASC_PROG_ERR,"Non-convergence in "
                        "non-linear solver at boundary");
#ifdef IDA_BND_DEBUG
                }else{
                    CONSOLE_DEBUG("Converged");
#endif
                }

                for (i = 0; i < num_bnds; i++) {
                    if (!rootsfound[i]) {
                        if ((bndman_real_eval(enginedata->bndlist[i]) > 0 && bnd_prev_eval[i] < 0) ||
                        (bndman_real_eval(enginedata->bndlist[i]) < 0 && bnd_prev_eval[i] > 0)) {
                            bnd_cond_states[i] = !bnd_cond_states[i];
                            if (bnd_prev_eval[i] > 0) {
                                bnd_set_ida_incr(enginedata->bndlist[i],0);
                                rootsfound[i] = -1;
                            }
                            else {
                                bnd_set_ida_incr(enginedata->bndlist[i],1);
                                rootsfound[i] = 1;
                            }
                            bnd_set_ida_value(enginedata->bndlist[i],bnd_cond_states[i]);
                        }
                    }
                }
                if (!ida_log_solve(integ,lrslv_ind)) return -1;

            }else events_triggered = 0;
            if (ida_bnd_reanalyse_cont(integ)) return 2;
            if (events_triggered) {
                integrator_output_write(integ);
                integrator_output_write_obs(integ);
                integrator_output_write_event(integ);
            }
        }
    }

    integrator_output_write(integ);
    integrator_output_write_obs(integ);
    integrator_output_write_event(integ);

    ASC_FREE(bnd_prev_eval);

    /* Reset the boundary flag */
    for (i = 0; i < num_bnds; i++)
        bnd_set_ida_crossed(enginedata->bndlist[i], 0);
    return 1;
}

1	#include "ida.h"
2	#include "idalinear.h"
3	#include "idaanalyse.h"
4	#include "idatypes.h"
5	#include "idaprec.h"
6	#include "idacalc.h"
7	#include "idaio.h"
8	#include "idaboundary.h"
9	#include <stdio.h>
10
11	#include <ascend/general/platform.h>
12	#include <ascend/general/list.h>
13	#include <ascend/general/panic.h>
14
15	#include <ascend/solver/solver.h>
16
17	#include <ascend/system/slv_client.h>
18	#include <ascend/system/cond_config.h>
19	#include <ascend/system/discrete.h>
20	#include <ascend/system/slv_types.h>
21	#include <ascend/system/slv_common.h>
22	#include <ascend/system/logrel.h>
23	#include <ascend/system/rel.h>
24	#include <ascend/system/discrete.h>
25	#include <ascend/system/var.h>
26	#include <ascend/system/block.h>
27	#include <ascend/system/bndman.h>
28
29	#define IDA_BND_DEBUG
30
31	/**
32	* Check to see if any of the system discrete variables have changed.
33	*
34	* @return 1 if any of the values have changed (does not necessarily mean
35	* the system needs to be reconfigured)
36	*/
37	int some_dis_vars_changed(slv_system_t sys) {
38	struct dis_discrete *dvlist, cur_dis;
39	int numDVs, i;
40	#ifdef IDA_BND_DEBUG
41	char *dis_name;
42	#endif
43
44	dvlist = slv_get_solvers_dvar_list(sys);
45	numDVs = slv_get_num_solvers_dvars(sys);
46
47	for (i = 0; i < numDVs; i++) {
48	cur_dis = dvlist[i];
49	if((dis_kind(cur_dis) == e_dis_boolean_t)
50	&& (dis_inwhen(cur_dis) \|\| dis_inevent(cur_dis))
51	){
52	if(dis_value(cur_dis) != dis_previous_value(cur_dis)){
53	#ifdef IDA_BND_DEBUG
54	dis_name = dis_make_name(sys, cur_dis);
55	CONSOLE_DEBUG("Boolean %s (i=%d) has changed (current=%d, prev=%d)", dis_name,
56	i, dis_value(cur_dis), dis_previous_value(cur_dis));
57	ASC_FREE(dis_name);
58	#endif
59	return 1;
60	}
61	}
62	}
63	CONSOLE_DEBUG("No boundary vars have changed");
64	return 0;
65
66	}
67
68	static void ida_write_values(IntegratorSystem *integ) {
69	struct var_variable **vl;
70	struct dis_discrete **dvl;
71	int32 c;
72	vl = slv_get_solvers_var_list(integ->system);
73	dvl = slv_get_solvers_dvar_list(integ->system);
74	for (c = 0; vl[c] != NULL; c++)
75	CONSOLE_DEBUG("Value of %s is %f", var_make_name(integ->system,vl[c]),var_value(vl[c]));
76	for (c = 0; dvl[c] != NULL; c++)
77	CONSOLE_DEBUG("Value of %s is %d", dis_make_name(integ->system,dvl[c]),dis_value(dvl[c]));
78	}
79
80	int ida_setup_lrslv(IntegratorSystem *integ, int qrslv_ind, int lrslv_ind) {
81	CONSOLE_DEBUG("Running logical solver...");
82	ida_log_solve(integ, lrslv_ind);
83	if(some_dis_vars_changed(integ->system)){
84	CONSOLE_DEBUG("Some discrete vars changed; reanalysing");
85	return ida_bnd_reanalyse_cont(integ);
86	}
87	return 0;
88	}
89
90	int ida_bnd_reanalyse(IntegratorSystem *integ){
91
92	if (integ->y_id != NULL) {
93	ASC_FREE(integ->y_id);
94	integ->y_id = NULL;
95	}
96
97	if (integ->obs_id != NULL){
98	ASC_FREE(integ->obs_id);
99	integ->obs_id = NULL;
100	}
101	if (integ->y != NULL) {
102	ASC_FREE(integ->y);
103	integ->y = NULL;
104	}
105	if (integ->ydot != NULL) {
106	ASC_FREE(integ->ydot);
107	integ->ydot = NULL;
108	}
109	if (integ->obs != NULL) {
110	ASC_FREE(integ->obs);
111	integ->obs = NULL;
112	}
113
114	integ->n_y = 0;
115
116
117	return reanalyze_solver_lists(integ->system);
118	}
119
120	int ida_bnd_reanalyse_cont(IntegratorSystem *integ){
121	CONSOLE_DEBUG("Clearing memory from previous calls...");
122	if (integ->y_id != NULL) {
123	ASC_FREE(integ->y_id);
124	integ->y_id = NULL;
125	}
126
127	if (integ->obs_id != NULL){
128	ASC_FREE(integ->obs_id);
129	integ->obs_id = NULL;
130	}
131	if (integ->y != NULL) {
132	ASC_FREE(integ->y);
133	integ->y = NULL;
134	}
135	if (integ->ydot != NULL) {
136	ASC_FREE(integ->ydot);
137	integ->ydot = NULL;
138	}
139	if (integ->obs != NULL) {
140	ASC_FREE(integ->obs);
141	integ->obs = NULL;
142	}
143
144	integ->n_y = 0;
145
146	return integrator_ida_analyse(integ);
147	}
148
149	int ida_bnd_update_relist(IntegratorSystem *integ){
150	IntegratorIdaData *enginedata;
151	struct rel_relation **rels;
152	int i,j,n_solverrels,n_active_rels;
153
154	enginedata = integrator_ida_enginedata(integ);
155
156	n_solverrels = slv_get_num_solvers_rels(integ->system);
157	n_active_rels = slv_count_solvers_rels(integ->system, &integrator_ida_rel);
158	rels = slv_get_solvers_rel_list(integ->system);
159
160	if(enginedata->rellist != NULL){
161	ASC_FREE(enginedata->rellist);
162	enginedata->rellist = NULL;
163	enginedata->rellist = ASC_NEW_ARRAY(struct rel_relation *, n_active_rels);
164	}
165
166	j = 0;
167	for (i = 0; i < n_solverrels; ++i) {
168	if (rel_apply_filter(rels[i], &integrator_ida_rel)) {
169	#ifdef IDA_BND_DEBUG
170	char *relname;
171	relname = rel_make_name(integ->system, rels[i]);
172	CONSOLE_DEBUG("rel '%s': 0x%x", relname, rel_flags(rels[i]));
173	ASC_FREE(relname);
174	#endif
175	enginedata->rellist[j++] = rels[i];
176	}
177	}
178	asc_assert(j == n_active_rels);
179	enginedata->nrels = n_active_rels;
180
181	if (enginedata->nrels != integ->n_y) {
182	ERROR_REPORTER_HERE(ASC_USER_ERROR
183	,"Integration problem is not square (%d active rels, %d vars)"
184	,n_active_rels, integ->n_y
185	);
186	return 1; /* failure */
187	}
188
189	return 0;
190	}
191
192	N_Vector ida_bnd_new_zero_NV(long int vec_length){
193	int i;
194	N_Vector nv = N_VNew_Serial(vec_length);
195	for(i= 0; i< vec_length; i++) {
196	NV_Ith_S(nv,i) = 0.0;
197	}
198
199	return nv;
200	}
201
202
203	void ida_bnd_update_IC(IntegratorSystem *integ, realtype t0, N_Vector y0, N_Vector yp0) {
204	/* First destroy since n_y may have changed */
205	N_VDestroy_Serial(y0);
206	N_VDestroy_Serial(yp0);
207	/* retrieve new initial values from the system */
208	t0 = integrator_get_t(integ);
209	y0 = ida_bnd_new_zero_NV(integ->n_y);
210	integrator_get_y(integ, NV_DATA_S(y0));
211
212	yp0 = ida_bnd_new_zero_NV(integ->n_y);
213	integrator_get_ydot(integ, NV_DATA_S(yp0));
214
215	#ifdef IDA_BND_DEBUG
216	CONSOLE_DEBUG("BEFORE IC SOLVING:");
217	CONSOLE_DEBUG("TIME: %f", t0);
218	CONSOLE_DEBUG("Y");
219	N_VPrint_Serial(y0);
220	CONSOLE_DEBUG("Yp");
221	N_VPrint_Serial(yp0);
222	CONSOLE_DEBUG("Press any to continue...");
223	getchar();
224	#endif
225
226	}
227
228	int ida_log_solve(IntegratorSystem *integ, int lrslv_ind) {
229	slv_parameters_t parameters;
230	int num_params;
231	char *pname;
232	slv_status_t status;
233	int i;
234	if (slv_select_solver(integ->system, lrslv_ind) == -1) {
235	ERROR_REPORTER_HERE(ASC_PROG_ERR,"Error attempting to load LRSlv");
236	}
237	#ifdef IDA_BND_DEBUG
238	CONSOLE_DEBUG("Solver selected is '%s'"
239	,slv_solver_name(slv_get_selected_solver(integ->system))
240	);
241	#endif
242
243	/* Flip LRSlv into ida mode */
244	slv_get_parameters(integ->system, &parameters);
245	num_params = parameters.num_parms;
246	for (i = 0; i<num_params; i++) {
247	pname = parameters.parms[i].name;
248	if(strcmp(pname, "withida") == 0) {
249	parameters.parms[i].info.b.value = 1;
250	}
251	}
252	/* solve the logical relations in the model, if possible */
253	slv_presolve(integ->system);
254	slv_solve(integ->system);
255
256	/* Check for convergence */
257	slv_get_status(integ->system, &status);
258	if (!status.converged) {
259	ERROR_REPORTER_HERE(ASC_PROG_ERR,"Non-convergence in logical solver.");
260	return 0;
261	#ifdef IDA_BND_DEBUG
262	}else{
263	CONSOLE_DEBUG("Converged");
264	#endif
265	}
266	return 1;
267	}
268
269	/*
270	* Uses LRSlv to check if any of the logical conditions in the model need
271	* updating after a boundary crossing. If so, the model is reconfigured and
272	* new initial conditions are determined at the point of the crossing.
273	*
274	* @return 1 if the crossing causes a change in the system
275	*/
276
277	int ida_cross_boundary(IntegratorSystem integ, int rootsfound,
278	int *bnd_cond_states, int qrslv_ind, int lrslv_ind) {
279
280	IntegratorIdaData *enginedata;
281	slv_status_t status;
282
283	struct bnd_boundary *bnd = NULL;
284	int i, num_bnds, num_dvars;
285
286	struct dis_discrete **dvl;
287	int32 c, *prevals = NULL;
288
289	double *bnd_prev_eval;
290
291	CONSOLE_DEBUG("Crossing boundary...");
292
293	integrator_output_write(integ);
294	integrator_output_write_obs(integ);
295	integrator_output_write_event(integ);
296	/^^^ FIXME should we write the above event? It may not have crossed in correct direction...? /
297
298	/* Flag the crossed boundary and update bnd_cond_states */
299	enginedata = integ->enginedata;
300	num_bnds = enginedata->nbnds;
301	bnd_prev_eval = ASC_NEW_ARRAY(double,num_bnds);
302	for(i = 0; i < num_bnds; i++) {
303	/* get the value of the boundary condition before crossing */
304	bnd_prev_eval[i] = bndman_real_eval(enginedata->bndlist[i]);
305	if(rootsfound[i]){
306	/* reminder: 'rootsfound[i] == 1 for UP or -1 for DOWN. */
307	/* this boundary was one of the boundaries triggered */
308	bnd = enginedata->bndlist[i];
309	#ifdef IDA_BND_DEBUG
310	char *name = bnd_make_name(integ->system,enginedata->bndlist[i]);
311	CONSOLE_DEBUG("Boundary '%s': ida_incr=%d, dirn=%d,ida_value=%d,ida_first_cross=%d,bnd_cond_states[i]=%d"
312	,name,bnd_ida_incr(bnd),rootsfound[i],bnd_ida_value(bnd)
313	,bnd_ida_first_cross(bnd)!=0,bnd_cond_states[i]
314	);
315	#endif
316	bnd_set_ida_crossed(bnd, 1);
317	if(bnd_ida_first_cross(bnd) /* not crossed before */
318	\|\| !((rootsfound[i] == 1 && bnd_ida_incr(bnd)) /* not crossing upwards twice in a row */
319	\|\| (rootsfound[i] == -1 && !bnd_ida_incr(bnd))) /* not crossing downwards twice in a row */
320	){
321	if(bnd_cond_states[i] == 0){
322	bnd_set_ida_value(bnd, 1);
323	bnd_cond_states[i] = 1;
324	}else{
325	bnd_set_ida_value(bnd, 0);
326	bnd_cond_states[i] = 0;
327	}
328	#ifdef IDA_BND_DEBUG
329	CONSOLE_DEBUG("Set boundary '%s' to %d (single cross)",name,bnd_ida_value(bnd)!=0);
330	#endif
331	}else{
332	/* Boundary crossed twice in one direction. Very unlikey! */
333
334	/* The aim of the following two lines is to set the value of
335	the boolean variable to such a value as if the boundary
336	was crossed in the opposite direction before */
337	CONSOLE_DEBUG("DOUBLE CROSS");
338	if(!prevals){
339	num_dvars = slv_get_num_solvers_dvars(integ->system);
340	dvl = slv_get_solvers_dvar_list(integ->system);
341	prevals = ASC_NEW_ARRAY(int32,num_dvars);
342	for(c = 0; dvl[c] != NULL; c++)
343	prevals[c] = dis_value(dvl[c]);
344	}
345	bnd_set_ida_value(bnd, !bnd_cond_states[i]);
346	if(!ida_log_solve(integ,lrslv_ind)){
347	CONSOLE_DEBUG("Error in logic solve in double-cross");
348	return -1;
349	}
350	bnd_set_ida_value(bnd,bnd_cond_states[i]);
351	#ifdef IDA_BND_DEBUG
352	CONSOLE_DEBUG("After double-cross, set boundary '%s' to %d",name,bnd_ida_value(bnd)?1:0);
353	#endif
354	}
355	/* flag this boundary as having previously been crossed */
356	bnd_set_ida_first_cross(bnd,0);
357	/* store this recent crossing-direction for future reference */
358	bnd_set_ida_incr(bnd,(rootsfound[i] > 0));
359	#ifdef IDA_BND_DEBUG
360	ASC_FREE(name);
361	#endif
362	}
363	}
364	if(!ida_log_solve(integ,lrslv_ind)) return -1;
365
366	/* If there was a double crossing, because of ida_log_solve the previous values
367	of discrete variables may be equal to their current values, which would mean that
368	the discrete vars haven't changed their values, but actually they did. So here we
369	restore the previous values of those variables, which are not connected with the
370	boundary which was double-crossed. */
371	if(prevals){
372	for(c = 0; dvl[c] != NULL; c++)
373	if(!(dis_value(dvl[c]) == prevals[c] && dis_value(dvl[c]) != dis_previous_value(dvl[c])))
374	dis_set_previous_value(dvl[c],prevals[c]);
375	ASC_FREE(prevals);
376	}
377
378	integrator_output_write(integ);
379	integrator_output_write_obs(integ);
380	integrator_output_write_event(integ);
381	if(!some_dis_vars_changed(integ->system)){
382	CONSOLE_DEBUG("Crossed boundary but no effect on system");
383	/* Boundary crossing that has no effect on system */
384	ASC_FREE(bnd_prev_eval);
385
386	/* Reset the boundary flag */
387	for(i = 0; i < num_bnds; i++)
388	bnd_set_ida_crossed(enginedata->bndlist[i], 0);
389	return 0;
390	}
391	/* update the main system if required */
392	int events_triggered = 1;
393	if(slv_get_num_solvers_events(integ->system)!=0) {
394	while(some_dis_vars_changed(integ->system) && events_triggered) {
395	if(ida_bnd_reanalyse(integ)) {
396	/* select QRSlv solver, and solve the system */
397	if(slv_select_solver(integ->system, qrslv_ind) == -1) {
398	ERROR_REPORTER_HERE(ASC_PROG_ERR,"Error attempting to load QRSlv");
399	}
400	#ifdef IDA_BND_DEBUG
401	{
402	struct var_variable **vl = slv_get_solvers_var_list(integ->system);
403	int i, n=slv_get_num_solvers_vars(integ->system), first=1;
404	CONSOLE_DEBUG("In boundary problem: variables (active, incident, free):");
405	for(i=0;i<n;++i){
406	if(var_incident(vl[i])&&!var_fixed(vl[i])&&var_active(vl[i])){
407	char *name = var_make_name(integ->system,vl[i]);
408	fprintf(stderr,"%s%s",(first?"\t":", "),name);
409	first=0; ASC_FREE(name);
410	}
411	}
412	fprintf(stderr,"\n");
413	}
414	{
415	struct rel_relation **rl = slv_get_solvers_rel_list(integ->system);
416	int i, n=slv_get_num_solvers_rels(integ->system), first=1;
417	CONSOLE_DEBUG("...relations (equality, included, active):");
418	for(i=0;i<n;++i){
419	if(rel_equality(rl[i])&&rel_active(rl[i])&&rel_included(rl[i])){
420	char *name = rel_make_name(integ->system,rl[i]);
421	fprintf(stderr,"%s%s",(first?"\t":", "),name);
422	first=0; ASC_FREE(name);
423	}
424	}
425	fprintf(stderr,"\n");
426	}
427	#endif
428	slv_presolve(integ->system);
429	slv_solve(integ->system);
430
431	slv_get_status(integ->system, &status);
432	if (!status.converged) {
433	ERROR_REPORTER_HERE(ASC_PROG_ERR,"Non-convergence in "
434	"non-linear solver at boundary");
435	#ifdef IDA_BND_DEBUG
436	}else{
437	CONSOLE_DEBUG("Converged");
438	#endif
439	}
440
441	for (i = 0; i < num_bnds; i++) {
442	if (!rootsfound[i]) {
443	if ((bndman_real_eval(enginedata->bndlist[i]) > 0 && bnd_prev_eval[i] < 0) \|\|
444	(bndman_real_eval(enginedata->bndlist[i]) < 0 && bnd_prev_eval[i] > 0)) {
445	bnd_cond_states[i] = !bnd_cond_states[i];
446	if (bnd_prev_eval[i] > 0) {
447	bnd_set_ida_incr(enginedata->bndlist[i],0);
448	rootsfound[i] = -1;
449	}
450	else {
451	bnd_set_ida_incr(enginedata->bndlist[i],1);
452	rootsfound[i] = 1;
453	}
454	bnd_set_ida_value(enginedata->bndlist[i],bnd_cond_states[i]);
455	}
456	}
457	}
458	if (!ida_log_solve(integ,lrslv_ind)) return -1;
459
460	}else events_triggered = 0;
461	if (ida_bnd_reanalyse_cont(integ)) return 2;
462	if (events_triggered) {
463	integrator_output_write(integ);
464	integrator_output_write_obs(integ);
465	integrator_output_write_event(integ);
466	}
467	}
468	}
469
470	integrator_output_write(integ);
471	integrator_output_write_obs(integ);
472	integrator_output_write_event(integ);
473
474	ASC_FREE(bnd_prev_eval);
475
476	/* Reset the boundary flag */
477	for (i = 0; i < num_bnds; i++)
478	bnd_set_ida_crossed(enginedata->bndlist[i], 0);
479	return 1;
480	}
481