//This is taken from the book, $UL,1$Numeric Recipies in C$UL,0$

Ode *OdeNew(U8 n)
{
  Ode *o=MAllocZ(sizeof(Ode));
  o->n=n;
  o->state=MAllocZ(n*sizeof(double));
  o->base_t=ReadSysTimer*1.0/SYS_TIMER_FREQ;
  o->h=1.0e-6;
  o->DstateDt=MAllocZ(n*sizeof(double));
  o->state_scale=MAllocZ(n*sizeof(double));
  o->initial_state=MAllocZ(n*sizeof(double));
  o->tolerance_start=1.0e-6;
  o->temp0=MAllocZ(n*sizeof(double));
  o->temp1=MAllocZ(n*sizeof(double));
  o->temp2=MAllocZ(n*sizeof(double));
  o->temp3=MAllocZ(n*sizeof(double));
  o->temp4=MAllocZ(n*sizeof(double));
  o->temp5=MAllocZ(n*sizeof(double));
  o->temp6=MAllocZ(n*sizeof(double));
  o->temp7=MAllocZ(n*sizeof(double));
  return o;
}

void OdeDel(Ode *o)
{
  Free(o->state);
  Free(o->DstateDt);
  Free(o->state_scale);
  Free(o->initial_state);
  Free(o->temp0);
  Free(o->temp1);
  Free(o->temp2);
  Free(o->temp3);
  Free(o->temp4);
  Free(o->temp5);
  Free(o->temp6);
  Free(o->temp7);
  Free(o);
}

void OdeOneStep(Ode *o)
{
  I8 i;
  o->derivative(o,o->t+o->base_t,o->state,o->DstateDt);
  for (i=0;i<o->n;i++)
    o->state[i]+=o->h*o->DstateDt[i];
  o->t+=o->h;
}

void OdeRK4OneStep(Ode *o)
{
  I8 i,n=o->n;
  double xh,hh,h6;
  double *dym,*dyt,*yt,*DstateDt;

  dym =o->temp0;
  dyt =o->temp1;
  yt  =o->temp2;
  DstateDt=o->temp3;
  hh  =0.5*o->h;
  h6  =o->h / 6.0;
  xh  =o->t +o->base_t+ hh;

  o->derivative(o,o->t+o->base_t,o->state,o->DstateDt);
  for (i=0;i<n;i++)
    yt[i]=o->state[i]+hh*DstateDt[i];
  o->derivative(o,xh,yt,dyt);
  for (i=0;i<n;i++)
    yt[i]=o->state[i]+hh*dyt[i];
  o->derivative(o,xh,yt,dym);
  for (i=0;i<n;i++) {
    yt[i]=o->state[i]+o->h*dym[i];
    dym[i]+=dyt[i];
  }
  o->t+=o->h;
  o->derivative(o,o->t+o->base_t,yt,dyt);
  for (i=0;i<n;i++)
    o->state[i]+=h6*(DstateDt[i]+dyt[i]+2.0*dym[i]);
}


double ODEa2=0.2, ODEa3=0.3, ODEa4=0.6, ODEa5=1.0, ODEa6=0.875, ODEb21=0.2,
	ODEb31=3.0/40.0, ODEb32=9.0/40.0, ODEb41=0.3, ODEb42=-0.9, ODEb43=1.2,
	ODEb51=-11.0/54.0, ODEb52=2.5, ODEb53=-70.0/27.0, ODEb54=35.0/27.0,
	ODEb61=1631.0/55296.0, ODEb62=175.0/512.0, ODEb63=575.0/13824.0,
	ODEb64=44275.0/110592.0, ODEb65=253.0/4096.0, ODEc1=37.0/378.0,
	ODEc3=250.0/621.0, ODEc4=125.0/594.0, ODEc6=512.0/1771.0,
	ODEdc1=37.0/378.0 -2825.0/27648.0,
	ODEdc3=250.0/621.0-18575.0/48384.0,
	ODEdc4=125.0/594.0-13525.0/55296.0,
	ODEdc5=-277.0/14336.0,
	ODEdc6=512.0/1771.0-0.25;



void OdeCashKarp(Ode *o)
{
  I8 i,n=o->n;
  double h=o->h,*state=o->state,*DstateDt=o->DstateDt,*ak2,*ak3,*ak4,*ak5,*ak6,*tempstate,*stateerr,*outstate;

  ak2=o->temp0;
  ak3=o->temp1;
  ak4=o->temp2;
  ak5=o->temp3;
  ak6=o->temp4;
  tempstate=o->temp5;
  outstate=o->temp6;
  stateerr=o->temp7;

  for (i=0;i<n;i++)
    tempstate[i]=state[i]+ODEb21*h*DstateDt[i];
  o->derivative(o,o->t+o->base_t+ODEa2*h,tempstate,ak2);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb31*DstateDt[i]+ODEb32*ak2[i]);
  o->derivative(o,o->t+o->base_t+ODEa3*h,tempstate,ak3);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb41*DstateDt[i]+ODEb42*ak2[i]+ODEb43*ak3[i]);
  o->derivative(o,o->t+o->base_t+ODEa4*h,tempstate,ak4);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb51*DstateDt[i]+ODEb52*ak2[i]+ODEb53*ak3[i]+ODEb54*ak4[i]);
  o->derivative(o,o->t+o->base_t+ODEa5*h,tempstate,ak5);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb61*DstateDt[i]+ODEb62*ak2[i]+ODEb63*ak3[i]+ODEb64*ak4[i]+ODEb65*ak5[i]);
  o->derivative(o,o->t+o->base_t+ODEa6*h,tempstate,ak6);
  for (i=0;i<n;i++)
    outstate[i]=state[i]+h*(ODEc1*DstateDt[i]+ODEc3*ak3[i]+ODEc4*ak4[i]+ODEc6*ak6[i]);
  for (i=0;i<n;i++)
    stateerr[i]=h*(ODEdc1*DstateDt[i]+ODEdc3*ak3[i]+ODEdc4*ak4[i]+ODEdc5*ak5[i]+ODEdc6*ak6[i]);
}


#define SAFETY (0.9)
#define PGROW  (-0.2)
#define PSHRNK (-0.25)
#define ERRCON (1.89e-4)


void OdeRK5OneStep(Ode *o)
{
  I8 i;
  double errmax,temp;
  double *tempstate=o->temp6,*stateerr=o->temp7;
  while (TRUE) {
    OdeCashKarp(o);
    errmax=0.0;
    for (i=0;i<o->n;i++) {
      temp=Abs(stateerr[i]/o->state_scale[i]);
      if (temp>errmax)
	errmax=temp;
    }
    errmax/=o->tolerance;
    if (errmax>1.0 && o->h>1.0e-64) {
      temp=o->h*SAFETY*errmax`PSHRNK;
      if (temp<0.1*o->h)
	o->h*=0.1;
      else
	o->h=temp;
    } else {
      o->t+=o->h;
      if (errmax>ERRCON)
	o->h*=SAFETY*errmax`PGROW;
      else
	o->h*=5.0;
      if (o->h>1.0e32)
	o->h=1.0e32;
      MemCpy(o->state,tempstate,sizeof(double)*o->n);
      break;
    }
  }
}