#include <fstream.h>
#include <iomanip.h>
#include <vector>
#include <math.h>

#define RK_order 4

// Chapter 9.1, p.412-413
// Taylor System 1
template <class Type>
void taysys1(ofstream& out, Type a, Type b, int n, Type x, Type y)
{
	runtype h, x1, y1, x2, y2, x3, y3, x4, y4, t;

	t = a;
	h = (b - a)/ n;
	for (int k = 1; k <= n; k++)
	{
		x1 = x - y + t*(2 - t*(1 + t));
		y1 = x + y + t*t*(-4 + t);
		x2 = x1 - y1 + 2 - t*(2 + 3*t);
		y2 = x1 + y1 + t*(-8 + 3*t);
		x3 = x2 - y2 - 2 - 6*t;
		y3 = x2 + y2 - 8 + 6*t;
		x4 = x3 - y3 - 6;
		y4 = x3 + y3 + 6;
		x = x + h*(x1 + h/2*(x2 + h/3*(x3 + h/4*x4)));
		y = y + h*(y1 + h/2*(y2 + h/3*(y3 + h/4*y4)));
		t += h;	
		out << setw(5)<< k  << " t = " << setw(10)<< t
		    <<"  x =" << setw(10)<< x << "  y =" << setw(10) << y<< endl;
	}
}


// Chapter 9.1, p.414
// Taylor System 2
template <class Type>
void taysys2(ofstream& out, Type a, Type b, int n, int nsteps)
{
	std::vector<Type> x(n+1, 0.0);
	std::vector< std::vector<Type> > d(n+1, std::vector<Type>(5, 0.0));
	Type h, t;
	t = 0.0;
	x[1] = 1.0;
	x[2] = 0.0;
	out << "  0   t = " << setw(10)<< t <<"  x[1] =";
	out << setw(10)<< x[1] << "  x[2] =" << setw(10) << x[2]<< endl; 

	h = (b - a)/nsteps;
	for (int k = 1; k <= nsteps; k++)
	{
		d[1][1] = x[1] - x[2] + t*(2 - t*(1 + t));
		d[2][1] = x[1] + x[2] + t*t*(-4 + t);
		d[1][2] = d[1][1] - d[2][1] + 2 - t*(2 + 3*t);
		d[2][2] = d[1][1] + d[2][1] + t*(-8 + 3*t);
		d[1][3] = d[1][2] - d[2][2] - 2 - 6*t;
		d[2][3] = d[1][2] + d[2][2] - 8 + 6*t;
		d[1][4] = d[1][3] - d[2][3] - 6;
		d[2][4] = d[1][3] + d[2][3] + 6;
		for (int i = 1; i <= n; i++)
			x[i] = x[i] + h*(d[i][1] + h/2*(d[i][2] + h/3*(d[i][3] + h/4*d[i][4])));
		t += h;
		out << setw(5)<< k  << " t = " << setw(10)<< t <<"  x[1] =";
		out << setw(10)<< x[1] << "  x[2] =" << setw(10) << x[2]<< endl;
	}
}

// Chapter 9.1, p.415-416
// Runge-Kutta System
template <class Type>
void xp_sys1(int n, Type t, std::vector<Type> x, std::vector<Type>& f)
{
	f[1] = x[1] - x[2] + t * (2 - t *(1 + t));
	f[2] = x[1] + x[2] - t * t * (4 - t);
}

template <class Type>
void rk4sys(ofstream& out, int n, Type h, std::vector<Type> x, int nsteps)
{
	int i, j;
	Type t;
	std::vector<Type> y(n+1, 0.0);
	std::vector< std::vector<Type> > K(RK_order+1, std::vector<Type>(n+1, 0.0));

	t = 0.0;

	out << "0 t = " << t << " x[1] = " << x[1] << " x[2] = " << x[2] << endl;
	for (j = 1; j <= nsteps; j++)
	{
		xp_sys1(n, t, x, K[1]);
		for (i = 1; i <= n; i++)
			y[i] = x[i] + 0.5 * h * K[1][i];
		xp_sys1(n, Type(t+0.5*h), y, K[2]);
		for (i = 1; i <= n; i++)
			y[i] = x[i] + 0.5 * h * K[2][i];
		xp_sys1(n, Type(t+0.5*h), y, K[3]);
		for (i = 1; i <= n; i++)
			y[i] = x[i] + h * K[3][i];
		xp_sys1(n, Type(t+h), y, K[4]);
		for (i = 1; i <= n; i++)
			x[i] = x[i] + (h/6) * (K[1][i] + 2 * K[2][i] + 2 * K[3][i] + K[4][i]);
		t = t + h;
		out << "j = " << j << ", t = " << t << ", x = " << x[1] << ", y = " << x[2] << endl;
    }
}


// Chapter 9.3, p.430-431
// Adams-Moulton, Runge-Kutta
template <class Type>
void xp_sys(std::vector<Type> x, std::vector<Type>& f)
{
	f[0] = 1.0;
	f[1] = x[2];
	f[2] = x[1] - x[3] - 9*pow(x[2],2) + pow(x[4],3) + 6*x[5] + 2*x[0];
	f[3] = x[4];
	f[4] = x[5];
	f[5] = x[5] - x[2] + exp(x[1]) - x[0];
}

template <class Type>
void am_sys (int& m, int n, Type h, Type& est, std::vector< std::vector<Type> >& z, std::vector< std::vector<Type> > f)
{
	std::vector<Type> s(n+1, 0.0);
	std::vector<Type> y(n+1, 0.0);
	std::vector<int> a(5, 0);
	std::vector<int> b(5, 0);

	a[1] = 55;	a[2] = -59;		a[3] = 37;	a[4] = -9;
	b[1] = 9;	b[2] = 19;		b[3] = -5;	b[4] = 1;
	int i, j, k, mp1;
	Type d, dmax;

	mp1 = (1 + m)%5;
	xp_sys (z[m], f[m]);
	for (i = 0; i <= n; i++)
		s[i] = 0;
	for (k = 1; k <= 4; k++)
	{
		j = (m - k + 6)%5;
		for (i = 0; i <= n; i++)
			s[i] += a[k]*f[j][i];
	}
	for (i = 0; i <= n; i++)
		y[i] = z[m][i] + h*s[i]/24;
	xp_sys (y, f[mp1]);
	for (i = 0; i <= n; i++)
		s[i] = 0;
	for (k = 1; k <= 4; k++)
	{
		j = (mp1 - k + 6)%5;
		for (i = 0; i <= n; i++)
			s[i] += b[k]*f[j][i];
	}
	for (i = 0; i <= n; i++)
		z[mp1][i] = z[m][i] + h*s[i]/24;
	m = mp1;
	dmax = 0;
	for (i = 0; i <= n; i++)
	{
		d = fabs(z[m][i] - y[i]) / fabs(z[m][i]);
		if (d > dmax)
		{
			dmax = d;
			j = i;
		}
	}
	est = 19*dmax/270.0;
}

template <class Type>
void rk_sys(int& m, int n, Type h, std::vector< std::vector<Type> >& z, std::vector< std::vector<Type> > f)
{
  /*modified from rk4sys */
	std::vector<Type> y(n+1, 0.0);
	std::vector< std::vector<Type> > g(RK_order+1, std::vector<Type>(n+1, 0.0));
	int i, mp1;

	mp1 = (1 + m)%5;
	xp_sys(z[m], f[m]);
	for (i = 0; i <= n; i++)
		y[i] = z[m][i] + h/2*f[m][i];
	xp_sys(y, g[1]);
	for (i = 0; i <= n; i++)
		y[i] = z[m][i] + h/2*g[1][i];
	xp_sys(y, g[2]);
	for (i = 0; i <= n; i++)
		y[i] = z[m][i] + h*g[2][i];
	xp_sys (y, g[3]);
	for (i = 0; i <= n; i++)
		z[mp1][i] = z[m][i] + h*(f[m][i] + 2*g[1][i] + 2*g[2][i] + g[3][i])/6;
	m = mp1;
}

template <class Type>
void amrk(ofstream& out, int n, Type h, std::vector<Type>& x, int nsteps)
{
	int i, k, m;
	std::vector< std::vector<Type> > z(RK_order+1, std::vector<Type>(n+1, 0.0)); 
	std::vector< std::vector<Type> > f(RK_order+1, std::vector<Type>(n+1, 0.0));
	Type est;

	m = 0;
	out << "h = " << h << endl;
	for (i = 0; i <= n; i++)
		out << "x[" << i << "] = " << x[i] << " " << endl;
	for (i = 0; i <= n; i++)
		z[m][i] = x[i];

	for (k = 1; k <= 3; k++)
	{
		rk_sys(m, n, h, z, f);
		out << "k = " << k << endl;
		for (i = 0; i <= n; i++)
			out << "z[" << m << "][" << i << "] = " << z[m][i] << endl;
	}

	for (k = 4; k <= nsteps; k++)
	{
		if (k > 40)
			exit (1);
		am_sys(m, n, h, est, z, f);
		out << "k = " << k << ", est = " << est << endl;
		for (i = 0; i <= n; i++)
			out << "z[" << m << "][" << i << "] = " << z[m][i] << endl;
	}

	for (i = 0; i <= n; i++)
		x[i] = z[m][i];
}