#include <iostream.h>
#include <math.h>
#include "DynamicalSystem.h"

DynamicalSystem::DynamicalSystem(int n)
{
	NVariable=n;
	Precision=1.e-4;
	Hmin=0.;
	Hestimate=1.e-1;
	//
	// private var
	//	
}
//==============================================================
void DynamicalSystem::nrerror(char * error_text)
{
        cerr<<"Numerical Recipes run-time error : "<<error_text<<"...now exiting to system...\n";
}
//===========================================================================
double *DynamicalSystem::vector(int N)
{
        double *v;
        v= new double [N];
        if (!v) nrerror("allocation failure in vector()");
 	return v;
}
//========================================
void DynamicalSystem::free_vector(double *v)
{
        if(v) delete v;
}

//===========================================================================
void  DynamicalSystem::rk4(double *y, double *dydx, double x, double h, double *yout)
{
        int i;
        double xh,hh,h6,*dym,*dyt,*yt;
        dym=vector(NVariable);
        dyt=vector(NVariable);
        yt=vector(NVariable);
        hh=h*0.5;
        h6=h/6.0;
        xh=x+hh;
        for (i=0;i<NVariable;i++) yt[i]=y[i]+hh*dydx[i];
        EDP(xh,yt,dyt);
        for (i=0;i<NVariable;i++) yt[i]=y[i]+hh*dyt[i];
        EDP(xh,yt,dym);
        for (i=0;i<NVariable;i++) {
                yt[i]=y[i]+h*dym[i];
                dym[i] += dyt[i];
        }
        EDP(x+h,yt,dyt);
        for (i=0;i<NVariable;i++)
                yout[i]=y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);
        free_vector(yt);
        free_vector(dyt);
        free_vector(dym);
}


//===========================================================================

#define PGROW -0.20
#define PSHRNK -0.25
#define FCOR 0.06666666         /* 1.0/15.0 */
#define SAFETY 0.9
#define ERRCON 6.0e-4


void  DynamicalSystem::rkqc(double *y, double *dydx, double *x, double htry, 
                double eps, double *yscal, double *hdid, double *hnext)
{
        int i;
        double xsav,hh,h,temp,errmax;
        double *dysav,*ysav,*ytemp;
        

        dysav=vector(NVariable);
        ysav=vector(NVariable);
        ytemp=vector(NVariable);
        xsav=(*x);
        for (i=0;i<NVariable;i++) {
                ysav[i]=y[i];
                dysav[i]=dydx[i];
        }
        h=htry;
        for (;;) {
                hh=0.5*h;
                rk4(ysav,dysav,xsav,hh,ytemp);
                *x=xsav+hh;
                EDP(*x,ytemp,dydx);
                rk4(ytemp,dydx,*x,hh,y);
                *x=xsav+h;
                if (*x == xsav) nrerror("Step size too small in routine RKQC");
                rk4(ysav,dysav,xsav,h,ytemp);
                errmax=0.0;
                for (i=0;i<NVariable;i++) {
                        ytemp[i]=y[i]-ytemp[i];
                        temp=fabs(ytemp[i]/yscal[i]);
                        if (errmax < temp) errmax=temp;
                }
                errmax /= eps;
                if (errmax <= 1.0) {
                        *hdid=h;
                        *hnext=(errmax > ERRCON ?
                                SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
                        break;
                }
                h=SAFETY*h*exp(PSHRNK*log(errmax));
        }
        for (i=0;i<NVariable;i++) y[i] += ytemp[i]*FCOR;
        free_vector(ytemp);
        free_vector(dysav);
        free_vector(ysav);
}

#undef PGROW
#undef PSHRNK
#undef FCOR
#undef SAFETY
#undef ERRCON


//===========================================================================

#define MAXSTP 10000
#define TINY 1.0e-30



int  DynamicalSystem::odeint(double *ystart, double x1, double x2, double eps,
                double h1, double hmin, int *nok, int *nbad)
{
        int nstp,i;
        double xsav,x,hnext,hdid,h;
        double *yscal,*y,*dydx;
       

        yscal=vector(NVariable);
        y=vector(NVariable);
        dydx=vector(NVariable);
        x=x1;
        h=(x2 > x1) ? fabs(h1) : -fabs(h1);
        *nok = (*nbad) =  0;
        for (i=0;i<NVariable;i++) y[i]=ystart[i];
        for (nstp=1;nstp<=MAXSTP;nstp++) {
                EDP(x,y,dydx);
                for (i=0;i<NVariable;i++)
                        yscal[i]=fabs(y[i])+fabs(dydx[i]*h)+TINY;
                
                if ((x+h-x2)*(x+h-x1) > 0.0) h=x2-x;
                rkqc(y,dydx,&x,h,eps,yscal,&hdid,&hnext);
                if (hdid == h) ++(*nok); else ++(*nbad);
                if ((x-x2)*(x2-x1) >= 0.0) {
                        for (i=0;i<NVariable;i++) ystart[i]=y[i];
                        
                        free_vector(dydx);
                        free_vector(y);
                        free_vector(yscal);
                        return 0;
                }
                if (fabs(hnext) <= hmin) nrerror("Step size too small in ODEINT");
                h=hnext;
        }
        nrerror("Too many steps in routine ODEINT");
        return 1;
}

#undef MAXSTP
#undef TINY


//=================================================================

void DynamicalSystem::RungeKutta(double *y, double t1,double t2)
{
 int nok,nbad;
 odeint(y, t1, t2,Precision,Hestimate,Hmin,&nok,&nbad);
                
}