```/* ============================================================================
* D Y N I B E X - Example for a first validated simulation : system 61 of vericomp
* ============================================================================
* Copyright   : ENSTA ParisTech
* License     : This program can be distributed under the terms of the GNU LGPL.
*               See the file COPYING.LESSER.
*
* Author(s)   : Julien Alexandre dit Sandretto and Alexandre Chapoutot
* Created     : Jul 18, 2014
* Sponsored   : This research benefited from the support of the "Chair Complex Systems Engineering - Ecole Polytechnique,
* THALES, DGA, FX, DASSAULT AVIATION, DCNS Research, ENSTA ParisTech, Telecom ParisTech, Fondation ParisTech and FDO ENSTA"
* ---------------------------------------------------------------------------- */

#include "ibex.h"
#include "vibes.cpp"

using namespace ibex;

void plot_simu(const simulation* sim)
{
std::list<solution_g>::const_iterator iterator_list;
for(iterator_list=sim->list_solution_g.begin();iterator_list!=sim->list_solution_g.end();iterator_list++) {
vibes::drawBox(iterator_list->time_j.lb(), iterator_list->time_j.ub(),
iterator_list->box_j1->operator[](1).lb(), iterator_list->box_j1->operator[](1).ub(), "red[red]");
}
}

int main(){

//number of variables
const int n= 5;
const int n1= 3;
const int n2= 3;

//number of shared variables
const int in1= 2;
const int in2= 2;

// full system variables
Variable y(n);
IntervalVector yinit(n);
IntervalVector picard(n);
yinit[0] = Interval(1);
yinit[1] = Interval(1);
yinit[2] = Interval(1);
yinit[3] = Interval(1);
yinit[4] = Interval(0);

// 1st sub system variables
Variable y1(n1);
IntervalVector yin1(n1);
IntervalVector yinit1(n1);
IntervalVector yin1_test(n1);
IntervalVector ypic1(n1);
yinit1[0] = Interval(1);
yinit1[1] = Interval(1);
yinit1[2] = Interval(0);
yin1[0] = Interval(1);
yin1[1] = Interval(1);
yin1[2] = Interval(0);

IntervalVector yin1_old(in2);
IntervalVector yin1_old_old(in2);

yin1_old = yin1;
yin1_old_old = yin1_old;

// 2nd sub system variables
Variable y2(n2);
IntervalVector yin2(n2);
IntervalVector yinit2(n2);
IntervalVector yin2_test(n2);
IntervalVector ypic2(n2);
yinit2[0] = Interval(1);
yinit2[1] = Interval(1);
yinit2[2] = Interval(0);
yin2[0] = Interval(1);
yin2[1] = Interval(1);
yin2[2] = Interval(0);

IntervalVector yin2_old(in2);
IntervalVector yin2_old_old(in2);

yin2_old = yin2;
yin2_old_old = yin2_old;

double m1, c1, d1, p1, m2, c2, d2, p2, cc, dc, H, h, T_max, Tn, Tnm1, Tnm2;
int nb_glob_step;
int pol_degree = 2;

Tn = 0;
Tnm1 = 0;
Tnm2 = 0;

m1 = 1;
c1 = 1;
d1 = 1;
p1 = 1;

m2 = 1;
c2 = 1;
d2 = 0;
p2 = 1;

cc = 1;
dc = 1;

// Cosimulation parameters
H = 0.05;
T_max = 2;
nb_glob_step = (int) T_max/H;

Function ydot = Function(y,Return( -c1*y[1]-d1*y[0]+cc*(y[3]-y[1])+dc*(y[2]-y[0]),
y[0],
-cc*(y[3]-y[1])-c2*y[3]-dc*(y[2]-y[0]),
y[2],
Interval(1.0)));

Function ydot1 = Function(y1,Return( -c1*y1[1]-d1*y1[0]+cc*(yin1[1]-y1[1])+dc*(yin1[0]-y1[0]),
y1[0],
y1[0]*0.0+1.0));

Function ydot2 = Function(y2,Return( -cc*(y2[1]-yin2[1])-c2*y2[1]-dc*(y2[0]-yin2[0]),
y2[0],
Interval(1.0)));

std::list<solution_j> list_sol1;
std::list<solution_j> list_sol2;

std::list<solution_j> list_sol1_temp;
std::list<solution_j> list_sol2_temp;

vibes::beginDrawing ();
vibes::newFigure("PDS");
vibes::setFigureProperty("plot","time",20);
vibes::setFigureProperty("plot","y",2);
vibes::setFigureProperty("plot","width",600);
vibes::setFigureProperty("plot","height",600);

for (int i=0; i<nb_glob_step; i++)
{

Tnm2 = Tnm1;
Tnm1 = Tn;
Tn = Tn+H;

int test = 0;

yin1_test[0] = yinit1[0];
yin1_test[1] = yinit1[1];
yin1_test[2] = Interval(Tn,Tn+H);

yin2_test[0] = yinit2[0];
yin2_test[1] = yinit2[1];
yin2_test[2] = Interval(Tn,Tn+H);

ypic1[0] = Interval::ALL_REALS;
ypic1[1] = Interval::ALL_REALS;
ypic1[2] = Interval::ALL_REALS;

ypic2[0] = Interval::ALL_REALS;
ypic2[1] = Interval::ALL_REALS;
ypic2[2] = Interval::ALL_REALS;

yin1 = yin2_test;
yin2 = yin1_test;

//Cross-Picard computation
while(((!ypic2.is_subset(yin2_test)) && (!ypic1.is_subset(yin1_test))) || (test<2))
{
yin1_test = ypic1;
yin2_test = ypic2;

test++;

Function ypic11 = Function(y1,Return( -c1*y1[1]-d1*y1[0]+cc*(yin1[1]-y1[1])+dc*(yin1[0]-y1[0]),
y1[0],
Interval(1.0)));

Function ypic22 = Function(y2,Return( -cc*(y2[1]-yin2[1])-c2*y2[1]-dc*(y2[0]-yin2[0]),
y2[0],
Interval(1.0)));

ivp_ode problem_pic1 = ivp_ode(ypic11,i*H,yinit1);
ivp_ode problem_pic2 = ivp_ode(ypic22,i*H,yinit2);

simulation simu_pic1 = simulation(&problem_pic1,H,HEUN,1e-8);
simulation simu_pic2 = simulation(&problem_pic2,H,HEUN,1e-8);

solution_j_heun u_j_pic1(Affine2Vector(yinit1,true),i*H,H, &problem_pic1,1e-4,1.0);
solution_j_heun u_j_pic2(Affine2Vector(yinit2,true),i*H,H, &problem_pic2,1e-4,1.0);

int test01 = u_j_pic1.calcul_j0(yinit1,&problem_pic1);
ypic1 = *u_j_pic1.box_j0;
int test02 = u_j_pic2.calcul_j0(yinit2,&problem_pic2);
ypic2 = *u_j_pic2.box_j0;

yin1 = ypic2;
yin2 = ypic1;

}

yin1 = ypic2;
yin2 = ypic1;

Function ypic11 = Function(y1,Return( -c1*y1[1]-d1*y1[0]+cc*(yin1[1]-y1[1])+dc*(yin1[0]-y1[0]),
y1[0],
Interval(1.0)));

Function ypic22 = Function(y2,Return( -cc*(y2[1]-yin2[1])-c2*y2[1]-dc*(y2[0]-yin2[0]),
y2[0],
Interval(1.0)));

ivp_ode problem_pic1 = ivp_ode(ypic11,i*H,yinit1);
ivp_ode problem_pic2 = ivp_ode(ypic22,i*H,yinit2);

simulation simu_pic1 = simulation(&problem_pic1,H,HEUN,1e-8);
simulation simu_pic2 = simulation(&problem_pic2,H,HEUN,1e-8);

solution_j_heun u_j_pic1(Affine2Vector(yinit1,true),i*H,H, &problem_pic1,1e-4,1.0);
solution_j_heun u_j_pic2(Affine2Vector(yinit2,true),i*H,H, &problem_pic2,1e-4,1.0);

int test01 = u_j_pic1.calcul_j0(yinit1,&problem_pic1);
int test02 = u_j_pic2.calcul_j0(yinit2,&problem_pic2);

int test001 = u_j_pic1.calcul_j1(&problem_pic1);
ypic1 = *u_j_pic1.box_j1;
int test002 = u_j_pic2.calcul_j1(&problem_pic2);
ypic2 = *u_j_pic2.box_j1;

picard[0] = ypic1[0];
picard[1] = ypic1[1];
picard[2] = ypic2[0];
picard[3] = ypic2[1];
picard[4] = ypic2[2];

yin1[0] = picard[2];
yin1[1] = picard[3];
yin1[2] = yinit[4];

yin2[0] = picard[0];
yin2[1] = picard[1];
yin2[2] = yinit[4];

Function ypic111 = Function(y1,Return( -c1*y1[1]-d1*y1[0]+cc*(yin1[1]-y1[1])+dc*(yin1[0]-y1[0]),
y1[0],
Interval(1.0)));

Function ypic222 = Function(y2,Return( -cc*(y2[1]-yin2[1])-c2*y2[1]-dc*(y2[0]-yin2[0]),
y2[0],
Interval(1.0)));

ivp_ode problem_pic11 = ivp_ode(ypic111,i*H,yinit1);
ivp_ode problem_pic22 = ivp_ode(ypic222,i*H,yinit2);

simulation simu_pic11 = simulation(&problem_pic11,H,HEUN,1e-8);
simulation simu_pic22 = simulation(&problem_pic22,H,HEUN,1e-8);

// Cosimulation step
if(i>1) // Interpolation used when past states are available
{

IntervalVector picard3d2 = problem_pic11.compute_derivatives(2,ypic1);

IntervalVector picard3d1 = problem_pic22.compute_derivatives(2,ypic2);

yin1 = yinit2;
yin2 = yinit1;

Function ydot11 = Function(y1,Return( -c1*y1[1]-d1*y1[0]+cc*(((yin1[1]*(y1[2]-Tnm1)/(Tn-Tnm1)*(y1[2]-Tnm2)/(Tn-Tnm2) + yin1_old[1]*(y1[2]-Tn)/(Tnm1-Tn)*(y1[2]-Tnm2)/(Tnm1-Tnm2) + yin1_old_old[1]*(y1[2]-Tn)/(Tnm2-Tn)*(y1[2]-Tnm1)/(Tnm2-Tnm1) + 1/6*picard3d1[1]*(y1[2]-Tn)*(y1[2]-Tnm1)*(y1[2]-Tnm2)))-y1[1])+dc*(((yin1[0]*(y1[2]-Tnm1)/(Tn-Tnm1)*(y1[2]-Tnm2)/(Tn-Tnm2) + yin1_old[0]*(y1[2]-Tn)/(Tnm1-Tn)*(y1[2]-Tnm2)/(Tnm1-Tnm2) + yin1_old_old[0]*(y1[2]-Tn)/(Tnm2-Tn)*(y1[2]-Tnm1)/(Tnm2-Tnm1) + 1/6*picard3d1[0]*(y1[2]-Tn)*(y1[2]-Tnm1)*(y1[2]-Tnm2)))-y1[0]),
y1[0],
Interval(1.0)));

Function ydot22 = Function(y2,Return( -cc*(y2[1]-((yin2[1]*(y2[2]-Tnm1)/(Tn-Tnm1)*(y2[2]-Tnm2)/(Tn-Tnm2) + yin2_old[1]*(y2[2]-Tn)/(Tnm1-Tn)*(y2[2]-Tnm2)/(Tnm1-Tnm2) + yin2_old_old[1]*(y2[2]-Tn)/(Tnm2-Tn)*(y2[2]-Tnm1)/(Tnm2-Tnm1) + 1/6*picard3d2[1]*(y2[2]-Tn)*(y2[2]-Tnm1)*(y2[2]-Tnm2))))-c2*y2[1]-dc*(y2[0]-((yin2[0]*(y2[2]-Tnm1)/(Tn-Tnm1)*(y2[2]-Tnm2)/(Tn-Tnm2) + yin2_old[0]*(y2[2]-Tn)/(Tnm1-Tn)*(y2[2]-Tnm2)/(Tnm1-Tnm2) + yin2_old_old[0]*(y2[2]-Tn)/(Tnm2-Tn)*(y2[2]-Tnm1)/(Tnm2-Tnm1) + 1/6*picard3d2[0]*(y2[2]-Tn)*(y2[2]-Tnm1)*(y2[2]-Tnm2)))),
y2[0],
Interval(1.0)));

ivp_ode problem1 = ivp_ode(ydot11,i*H,yinit1);
simulation simu1 = simulation(&problem1,(i+1)*H,HEUN,1e-6);
simu1.run_simulation();

ivp_ode problem2 = ivp_ode(ydot22,i*H,yinit2);
simulation simu2 = simulation(&problem2,(i+1)*H,HEUN,1e-6);
simu2.run_simulation();

yin1_old_old = yin1_old;
yin2_old_old = yin2_old;
yin1_old = yin1;
yin2_old = yin2;

yinit1 = simu1.get_last_aff();
yinit2 = simu2.get_last_aff();

plot_simu(&simu1);
plot_simu(&simu2);
}

else // No interpolation based on past states for the first steps
{
yin1[0] = picard[2];
yin1[1] = picard[3];
yin1[2] = yinit[4];

yin2[0] = picard[0];
yin2[1] = picard[1];
yin2[2] = yinit[4];

Function ydot1 = Function(y1,Return( -c1*y1[1]-d1*y1[0]+cc*(yin1[1]-y1[1])+dc*(yin1[0]-y1[0]),
y1[0],
Interval(1.0)));

Function ydot2 = Function(y2,Return( -cc*(y2[1]-yin2[1])-c2*y2[1]-dc*(y2[0]-yin2[0]),
y2[0],
Interval(1.0)));

ivp_ode problem1 = ivp_ode(ydot1,i*H,yinit1);
simulation simu1 = simulation(&problem1,(i+1)*H,HEUN,1e-6);
simu1.run_simulation();

ivp_ode problem2 = ivp_ode(ydot2,i*H,yinit2);
simulation simu2 = simulation(&problem2,(i+1)*H,HEUN,1e-6);
simu2.run_simulation();

yin1_old_old = yin1_old;
yin2_old_old = yin2_old;
yin1_old = yin1;
yin2_old = yin2;

yinit1 = simu1.get_last_aff();
yinit2 = simu2.get_last_aff();

plot_simu(&simu1);
plot_simu(&simu2);
}
}

vibes::endDrawing();

return 0;

}
```