/* ============================================================================
 * 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;

}