/* Steven Andrews, 1/28/97 */
/* modified 11/99 */
/* Added code called '2004 version' during 6/04.  It is separate from rest of code. */
/* See documentation called dynsys doc */
/* Copyright 2003 by Steven Andrews.  Permission is granted
   for non-commercial use of and modifications to the code. */

#include "dynsys.h"
#include "Plot.h"
#include "Utility.h"
#include "Rn.h"
#include "DiskIO.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <stddef.h>

#define Pmax 10


int ODEeuler(void (*eqm)(float *,float *,float *),float *k,int p,float *u,float tf,float dt,int (*trackfn)(float,int,float *)) {
	float dudt[Pmax],t;
	int i,z=0;

	for(t=0;t<tf;t+=dt)	{
		(*eqm)(u,k,dudt);
		for(i=0;i<p;i++)	u[i]+=dudt[i]*dt;
		if(trackfn)
			if((z=trackfn(t,p,u))!=0) break; }
	return z; }


int ODErk4(void (*eqm)(float *,float *,float *),float *k,int p,float *u,float tf,float dt,int (*trackfn)(float,int,float *)) {
	float ut[Pmax],k1[Pmax],k2[Pmax],k3[Pmax],k4[Pmax];
	float t,dt2,dt3,dt6;
	int i,z=0;
	
	dt2=dt*0.5;
	dt3=dt/3.0;
	dt6=dt/6.0;
	for(t=0;t<tf;t+=dt)	{
		(*eqm)(u,k,k1);
		for(i=0;i<p;i++)	ut[i]=u[i]+k1[i]*dt2;
		(*eqm)(ut,k,k2);
		for(i=0;i<p;i++)	ut[i]=u[i]+k2[i]*dt2;
		(*eqm)(ut,k,k3);
		for(i=0;i<p;i++)	ut[i]=u[i]+k3[i]*dt;
		(*eqm)(ut,k,k4);
		for(i=0;i<p;i++)	u[i]+=(k1[i]+k4[i])*dt6+(k2[i]+k3[i])*dt3;
		if(trackfn)
			if((z=trackfn(t,p,u))!=0) break; }
	return z; }


int ODErkas(void (*eqm)(float *,float *,float *),float *k,int p,float *u,float tf,float dt,int (*trackfn)(float,int,float *)) {
	float ut[Pmax],k1[Pmax],k2[Pmax],k3[Pmax],k4[Pmax],k5[Pmax];
	float ubig[Pmax],usmall[Pmax],uscal[Pmax],eps,err,diff,safe;
	float t,dt2,dt3,dt4,dt6,dt12;
	int i,z=0;
	
	(*eqm)(u,k,k1);												/* define a scaling vector */
	for(i=0;i<p;i++)	uscal[i]=fabs(u[i])+fabs(dt*k1[i]);
	eps=0;
	safe=0.95;														/* safety factor */
	
	for(t=0;t<tf;) {
		dt2=dt*0.5;
		dt3=dt/3.0;
		dt4=dt/4.0;
		dt6=dt/6.0;
		dt12=dt/12.0;
		
		for(i=0;i<p;i++)	ut[i]=u[i]+k1[i]*dt2;	/* take size dt step	*/
		(*eqm)(ut,k,k2);
		for(i=0;i<p;i++)	ut[i]=u[i]+k2[i]*dt2;
		(*eqm)(ut,k,k3);
		for(i=0;i<p;i++)	ut[i]=u[i]+k3[i]*dt;
		(*eqm)(ut,k,k4);
		for(i=0;i<p;i++)	ubig[i]=u[i]+(k1[i]+k4[i])*dt6+(k2[i]+k3[i])*dt3;
		
		for(i=0;i<p;i++)	ut[i]=u[i]+k1[i]*dt4;	/* take size dt2 step	*/
		(*eqm)(ut,k,k2);
		for(i=0;i<p;i++)	ut[i]=u[i]+k2[i]*dt4;
		(*eqm)(ut,k,k3);
		for(i=0;i<p;i++)	ut[i]=u[i]+k3[i]*dt2;
		(*eqm)(ut,k,k4);
		for(i=0;i<p;i++)	usmall[i]=u[i]+(k1[i]+k4[i])*dt12+(k2[i]+k3[i])*dt6;
		
		(*eqm)(usmall,k,k5);								/* take second dt2 step	*/
		for(i=0;i<p;i++)	ut[i]=usmall[i]+k5[i]*dt4;
		(*eqm)(ut,k,k2);
		for(i=0;i<p;i++)	ut[i]=usmall[i]+k2[i]*dt4;
		(*eqm)(ut,k,k3);
		for(i=0;i<p;i++)	ut[i]=usmall[i]+k3[i]*dt2;
		(*eqm)(ut,k,k4);
		for(i=0;i<p;i++)	usmall[i]+=(k5[i]+k4[i])*dt12+(k2[i]+k3[i])*dt6;
		
		err=0;															/* compute error	*/
		for(i=0;i<p;i++) {
			diff=fabs(usmall[i]-ubig[i])/uscal[i];
			err=(err>diff)?err:diff; }
		
		if(!eps) eps=err;
		if(err<=eps) {
			t+=dt;
			for(i=0;i<p;i++) u[i]=usmall[i];
			dt=safe*dt*pow(eps/err,0.25);
			if(t+dt>tf) dt=tf-t;
			if(trackfn)
				if((z=trackfn(t,p,u))!=0) break;
			(*eqm)(u,k,k1); }
		else
			dt=safe*dt*pow(eps/err,0.2); }
	return z; }


/* k value is omega^2 (=k/m) */
void EQMsho(float *u,float *k,float *dudt) {
	dudt[0]=u[1];
	dudt[1]=-k[0]*u[0]; }


/* k values are: sigma, r, b. Suggested values: 10, 28, 8/3 */
void EQMlorenz(float *u,float *k,float *dudt) {
	dudt[0]=k[0]*(u[1]-u[0]);
	dudt[1]=k[1]*u[0]-u[1]-u[0]*u[2];
	dudt[2]=u[0]*u[1]-k[2]*u[2]; }


phptr phptalloc(int sp,int fp,int fs) {
	phptr u;
	int ok;

	ok=1;
	u=(phptr) malloc(sizeof(struct phpt));
	ok=ok&&u;
	if(!ok) return 0;
	u->sp=sp;
	u->fp=fp;
	u->fs=fs;
	u->sa=u->fa=NULL;
	if(sp)	{u->sa=allocV(sp);ok=ok&&u->sa;}
	if(fp&&fs)	{u->fa=allocM(fs,fp);ok=ok&&u->fa;}
	if(ok) return u;
	else {phptfree(u);return 0; }}


void phptfree(phptr u) {
	if(!u) return;
	if(u->sa) freeV(u->sa);
	if(u->fa) freeM(u->fa);
	free(u); }


int phptsave(phptr u,char *fnames,char *fnamef) {
	int ok;
	
	ok=1;
	if(u->sp)	ok=ok&&SaveData(u->sa,u->sp,1,fnames,0);
	if(u->fp&&u->fs)	ok=ok&&SaveData(u->fa,u->fs,u->fp,fnamef,0);
	return ok; }


phptr phptload(char *fnames,char *fnamef) {
	phptr u;
	int m,n;
	float *a;

	u=phptalloc(0,0,0);
	if(!u) return NULL;
	if(LoadData2(&a,&m,&n,fnames,0)) {
		u->sp=m;
		u->sa=a; }
	if(LoadData2(&a,&m,&n,fnamef,0)) {
		u->fs=m;
		u->fp=n;
		u->fa=a; }
	return u; }


int ODEFeuler(void (*eqm)(phptr,void *,phptr),void *k,phptr u,float *Dt,float intpar[],int (*trackfn)(float,phptr,void *),void *trackptr) {
	phptr dudt;
	float *s,*f,*dsdt,*dfdt;
	float t,tf,dt;
	unsigned int sp,fp,fs;
	int i,j,z=0;

	sp=u->sp;
	fp=u->fp;
	fs=u->fs;
	s=u->sa;
	f=u->fa;
	dudt=phptalloc(sp,fp,fs);
	if(!dudt) return 1;
	dsdt=dudt->sa;
	dfdt=dudt->fa;
	dt=intpar[0];
	tf=*Dt;
	if(trackfn)	trackfn(0,u,trackptr);
	for(t=0;t<tf;t+=dt) {
		(*eqm)(u,k,dudt);
		for(i=0;i<sp;i++)	s[i]+=dsdt[i]*dt;
		for(j=0;j<fp;j++)	for(i=0;i<fs;i++)	f[fp*i+j]+=dfdt[fp*i+j]*dt;
		if(trackfn)
			if((z=trackfn(t,u,trackptr))!=0)	{t+=dt;break;}}
	*Dt=t;
	phptfree(dudt);
	return z; }


int ODEFrk4(void (*eqm)(phptr,void *,phptr),void *k,phptr u,float *Dt,float intpar[],int (*trackfn)(float,phptr,void *),void *trackptr) {
	phptr ut,k1,k2,k3,k4;
	unsigned int sp,fp,fs;
	float *s,*st,*sk1,*sk2,*sk3,*sk4;
	float *f,*ft,*fk1,*fk2,*fk3,*fk4;
	float t,tf,dt,dt2,dt3,dt6;
	int i,j,z=0;

	sp=u->sp;
	fp=u->fp;
	fs=u->fs;
	s=u->sa;
	f=u->fa;
	ut=phptalloc(sp,fp,fs);
	k1=phptalloc(sp,fp,fs);
	k2=phptalloc(sp,fp,fs);
	k3=phptalloc(sp,fp,fs);
	k4=phptalloc(sp,fp,fs);
	if(!(ut&&k1&&k2&&k3&&k4)) return 1;
	st=ut->sa;
	ft=ut->fa;
	sk1=k1->sa;
	fk1=k1->fa;
	sk2=k2->sa;
	fk2=k2->fa;
	sk3=k3->sa;
	fk3=k3->fa;
	sk4=k4->sa;
	fk4=k4->fa;
	dt=intpar[0];
	dt2=dt/2.0;
	dt3=dt/3.0;
	dt6=dt/6.0;
	tf=*Dt;
	if(trackfn)	trackfn(0,u,trackptr);
	
	for(t=0;t<tf;t+=dt)	{
		(*eqm)(u,k,k1);
		for(i=0;i<sp;i++) st[i]=s[i]+sk1[i]*dt2;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk1[fp*i+j]*dt2;
		(*eqm)(ut,k,k2);
		for(i=0;i<sp;i++) st[i]=s[i]+sk2[i]*dt2;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk2[fp*i+j]*dt2;
		(*eqm)(ut,k,k3);
		for(i=0;i<sp;i++) st[i]=s[i]+sk3[i]*dt;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk3[fp*i+j]*dt;
		(*eqm)(ut,k,k4);
		for(i=0;i<sp;i++) s[i]+=(sk1[i]+sk4[i])*dt6+(sk2[i]+sk3[i])*dt3;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) f[fp*i+j]+=(fk1[fp*i+j]+fk4[fp*i+j])*dt6+(fk2[fp*i+j]+fk3[fp*i+j])*dt3;

		if(trackfn)
			if((z=trackfn(t,u,trackptr))!=0)	{t+=dt;break;}}
	*Dt=t;
	phptfree(ut);
	phptfree(k1);
	phptfree(k2);
	phptfree(k3);
	phptfree(k4);
	return z; }


int ODEFrkas(void (*eqm)(phptr,void *,phptr),void *k,phptr u,float *Dt,float intpar[],int (*trackfn)(float,phptr,void *),void *trackptr) {
	phptr ut,k1,k2,k3,k4,k5;
	phptr ubig,usmall,uscal;
	unsigned int sp,fp,fs;
	float *s,*st,*sk1,*sk2,*sk3,*sk4,*sk5,*sbig,*ssmall,*sscal;
	float *f,*ft,*fk1,*fk2,*fk3,*fk4,*fk5,*fbig,*fsmall,*fscal;
	float eps,err,sum,diff,safe;
	float t,tf,dt,dt2,dt3,dt4,dt6,dt12;
	int i,j,z=0;

	sp=u->sp;
	fp=u->fp;
	fs=u->fs;
	s=u->sa;
	f=u->fa;
	ut=phptalloc(sp,fp,fs);
	k1=phptalloc(sp,fp,fs);
	k2=phptalloc(sp,fp,fs);
	k3=phptalloc(sp,fp,fs);
	k4=phptalloc(sp,fp,fs);
	k5=phptalloc(sp,fp,fs);
	ubig=phptalloc(sp,fp,fs);
	usmall=phptalloc(sp,fp,fs);
	uscal=phptalloc(sp,fp,fs);
	if(!(ut&&k1&&k2&&k3&&k4&&k5&&ubig&&usmall&&uscal)) return 1;
	st=ut->sa;
	ft=ut->fa;
	sk1=k1->sa;
	fk1=k1->fa;
	sk2=k2->sa;
	fk2=k2->fa;
	sk3=k3->sa;
	fk3=k3->fa;
	sk4=k4->sa;
	fk4=k4->fa;
	sk5=k5->sa;
	fk5=k5->fa;
	sbig=ubig->sa;
	fbig=ubig->fa;
	ssmall=usmall->sa;
	fsmall=usmall->fa;
	sscal=uscal->sa;
	fscal=uscal->fa;

	tf=*Dt;
	dt=intpar[0];
	
	(*eqm)(u,k,k1);										/* define scaling vector */
	for(i=0;i<sp;i++) sscal[i]=fabs(s[i])+fabs(dt*sk1[i]);
	for(j=0;j<fp;j++)	{
		sum=0;
		for(i=0;i<fs;i++) sum+=fabs(f[fp*i+j])+fabs(dt*fk1[fp*i+j]);
		sum/=fs;
		for(i=0;i<fs;i++) fscal[fp*i+j]=sum; }
	eps=0;
	safe=0.95;
	if(trackfn)	trackfn(0,u,trackptr);
	
	for(t=0;t<tf;) {
		dt2=dt/2.0;
		dt3=dt/3.0;
		dt4=dt/4.0;
		dt6=dt/6.0;
		dt12=dt/12.0;

		for(i=0;i<sp;i++) st[i]=s[i]+sk1[i]*dt2;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk1[fp*i+j]*dt2;
		(*eqm)(ut,k,k2);
		for(i=0;i<sp;i++) st[i]=s[i]+sk2[i]*dt2;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk2[fp*i+j]*dt2;
		(*eqm)(ut,k,k3);
		for(i=0;i<sp;i++) st[i]=s[i]+sk3[i]*dt;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk3[fp*i+j]*dt;
		(*eqm)(ut,k,k4);
		for(i=0;i<sp;i++) sbig[i]=s[i]+(sk1[i]+sk4[i])*dt6+(sk2[i]+sk3[i])*dt3;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) fbig[fp*i+j]=f[fp*i+j]+(fk1[fp*i+j]+fk4[fp*i+j])*dt6+(fk2[fp*i+j]+fk3[fp*i+j])*dt3;

		for(i=0;i<sp;i++) st[i]=s[i]+sk1[i]*dt4;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk1[fp*i+j]*dt4;
		(*eqm)(ut,k,k2);
		for(i=0;i<sp;i++) st[i]=s[i]+sk2[i]*dt4;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk2[fp*i+j]*dt4;
		(*eqm)(ut,k,k3);
		for(i=0;i<sp;i++) st[i]=s[i]+sk3[i]*dt2;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=f[fp*i+j]+fk3[fp*i+j]*dt2;
		(*eqm)(ut,k,k4);
		for(i=0;i<sp;i++) ssmall[i]=s[i]+(sk1[i]+sk4[i])*dt12+(sk2[i]+sk3[i])*dt6;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) fsmall[fp*i+j]=f[fp*i+j]+(fk1[fp*i+j]+fk4[fp*i+j])*dt12+(fk2[fp*i+j]+fk3[fp*i+j])*dt6;

		(*eqm)(usmall,k,k5);
		for(i=0;i<sp;i++) st[i]=ssmall[i]+sk5[i]*dt4;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=fsmall[fp*i+j]+fk5[fp*i+j]*dt4;
		(*eqm)(ut,k,k2);
		for(i=0;i<sp;i++) st[i]=ssmall[i]+sk2[i]*dt4;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=fsmall[fp*i+j]+fk2[fp*i+j]*dt4;
		(*eqm)(ut,k,k3);
		for(i=0;i<sp;i++) st[i]=ssmall[i]+sk3[i]*dt2;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) ft[fp*i+j]=fsmall[fp*i+j]+fk3[fp*i+j]*dt2;
		(*eqm)(ut,k,k4);
		for(i=0;i<sp;i++) ssmall[i]+=(sk5[i]+sk4[i])*dt12+(sk2[i]+sk3[i])*dt6;
		for(j=0;j<fp;j++) for(i=0;i<fs;i++) fsmall[fp*i+j]+=(fk5[fp*i+j]+fk4[fp*i+j])*dt12+(fk2[fp*i+j]+fk3[fp*i+j])*dt6;

		err=0;
		for(i=0;i<sp;i++) {
			diff=fabs(ssmall[i]-sbig[i])/sscal[i];
			err=(err>diff)?err:diff; }
		for(j=0;j<fp;j++) for(i=0;i<fs;i++)	{
			diff=fabs(fsmall[fp*i+j]-fbig[fp*i+j])/fscal[fp*i+j];
			err=(err>diff)?err:diff; }

		if(!eps) eps=err;
		if(err<=eps) {
			t+=dt;
			for(i=0;i<sp;i++) s[i]=ssmall[i];
			for(j=0;j<fp;j++) for(i=0;i<fs;i++) f[fp*i+j]=fsmall[fp*i+j];
			dt=safe*dt*pow(eps/err,0.25);
			if(t+dt>tf) dt=tf-t;
			if(trackfn)
				if((z=trackfn(t,u,trackptr))!=0)	break;
			(*eqm)(u,k,k1); }
		else
			dt=safe*dt*pow(eps/err,0.2); }
	
	*Dt=t;
	phptfree(ut);
	phptfree(k1);
	phptfree(k2);
	phptfree(k3);
	phptfree(k4);
	phptfree(k5);
	phptfree(ubig);
	phptfree(usmall);
	phptfree(uscal);
	return z; }


// all derivatives are 0
void EQMFzero(phptr u,void *k,phptr dudt) {
	int i,j,fp;
	
	k=NULL;												// to avoid warning
	fp=u->fp;
	for(i=0;i<dudt->sp;i++)	dudt->sa[i]=0;
	for(j=0;j<dudt->fp;j++)	for(i=0;i<dudt->fs;i++)	dudt->fa[fp*i+j]=0;
	return; }


// k[0] is w^2; sa[0] is position, sa[1] is velocity
void EQMFsho(phptr u,void *k,phptr dudt) {
	dudt->sa[0]=u->sa[1];
	dudt->sa[1]=-((float *)k)[0]*u->sa[0];
	return; }


// diffusion equation; k[0] is diffusion constant; u[row,0]=concentration; u->fp=1
void EQMFdiff(phptr u,void *k,phptr dudt) {
	int i,m;
	float alpha2;
	
	alpha2=((float *)k)[0];
	m=u->fs;
	dudt->fa[0]=alpha2*(u->fa[1]-u->fa[0]);
	for(i=1;i<m-1;i++)
		dudt->fa[i]=alpha2*(u->fa[i+1]+u->fa[i-1]-2*u->fa[i]);
	dudt->fa[m-1]=alpha2*(u->fa[m-2]-u->fa[m-1]);
	return; }


// wave equation on periodic string
// k[0]=c^2; u[row,0]=position, u[row,1]=velocity; u->fp=2
void EQMFwave(phptr u,void *k,phptr dudt) {
	int i,m;
	float c2;
	
	c2=((float *)k)[0];
	m=u->fs;
	dudt->fa[0]=u->fa[1];
	dudt->fa[1]=c2*(u->fa[2]+u->fa[2*(m-1)]-2*u->fa[0]);
	for(i=1;i<m-1;i++) {
		dudt->fa[2*i]=u->fa[2*i+1];
		dudt->fa[2*i+1]=c2*(u->fa[2*(i+1)]+u->fa[2*(i-1)]-2*u->fa[2*i]); }
	dudt->fa[2*(m-1)]=u->fa[2*(m-1)+1];
	dudt->fa[2*(m-1)+1]=c2*(u->fa[0]+u->fa[2*(m-2)]-2*u->fa[2*(m-1)]);
	return; }


/* displays first two scalers		*/
int TKFticker(float t,phptr u,void *tkptr) {
	tkptr=NULL;											// to avoid warning
	printf("%f\t%f\t%f\n",t,u->sa[0],u->sa[1]);
	return inkey(); }


/* plots first scaler						*/
int TKFtimeplot(float t,phptr u,void *tkptr) {
	tkptr=NULL;											// to avoid warning
	PlotPt(t,u->sa[0]);
	return inkey(); }


/* plots first field						*/
/* tkptr is float[], [0]=xmin, [1]=xmax, [2]=ymin, [3]=ymax	*/
int TKFshowfield(float t,phptr u,void *tkptr) {
	int i,fp;
	float xmin,xmax,ymin,ymax,dx;
	
	t=0;																// to avoid warning
	fp=u->fp;
	xmin=((float *)tkptr)[0];
	xmax=((float *)tkptr)[1];
	ymin=((float *)tkptr)[2];
	ymax=((float *)tkptr)[3];
	dx=(xmax-xmin)/u->fs;
	SetScales(xmin,xmax,ymin,ymax);
	PlotClear();
	DrawAxes();
	PlotPt(xmin,u->fa[0]);
	for(i=1;i<u->fs;i++) PlotLine(xmin+i*dx,u->fa[fp*i]);
	return inkey(); }


/******************** Start of 2004 version *****************************/

odeptr allocodestruct(int dim,int order,float *dtptr,void *systemptr,int (*eqm)(void *)) {
	int i;
	odeptr ode;

	if(!(order==1||order==2||order==4||order==5)) return NULL;
	ode=(odeptr) malloc(sizeof(struct odestruct));
	if(!ode) return NULL;
	ode->dim=dim;
	ode->order=order;
	ode->dtptr=dtptr;
	ode->dtsugg=0;
	ode->dtmax=0;
	ode->eps=0;
	ode->systemptr=systemptr;
	ode->eqm=eqm;
	ode->state0=ode->state1=NULL;
	ode->scale=NULL;
	ode->k1=ode->k2=ode->k3=ode->k4=NULL;
	ode->state0=(float**) calloc(dim,sizeof(float*));
	ode->state1=(float**) calloc(dim,sizeof(float*));
	if(!ode->state0||!ode->state1) {freeodestruct(ode);return NULL;}
	for(i=0;i<dim;i++) ode->state0[i]=ode->state1[i]=NULL;
	if(order>=2) {
		ode->k1=(float *) calloc(dim,sizeof(float));
		if(!ode->k1) {freeodestruct(ode);return NULL;}
		for(i=0;i<dim;i++) ode->k1[i]=0; }
	if(order>=4) {
		ode->k2=(float *) calloc(dim,sizeof(float));
		if(!ode->k2) {freeodestruct(ode);return NULL;}
		for(i=0;i<dim;i++) ode->k2[i]=0; }
	if(order>=5) {
		ode->scale=(float*) calloc(dim,sizeof(float));
		if(!ode->scale) {freeodestruct(ode);return NULL;}
		for(i=0;i<dim;i++) ode->scale[i]=1.0;
		ode->k3=(float *) calloc(dim,sizeof(float));
		if(!ode->k3) {freeodestruct(ode);return NULL;}
		ode->k4=(float *) calloc(dim,sizeof(float));
		if(!ode->k4) {freeodestruct(ode);return NULL;}
		for(i=0;i<dim;i++) ode->k3[i]=ode->k4[i]=0; }
	return ode; }


void freeodestruct(odeptr ode) {
	if(!ode) return;
	if(ode->state0) free(ode->state0);
	if(ode->state1) free(ode->state1);
	if(ode->scale) free(ode->scale);
	if(ode->k1) free(ode->k1);
	if(ode->k2) free(ode->k2);
	free(ode);
	return; }


int runodestruct(odeptr ode) {
	int dim,order,i,er;
	static int redoctr=0;
	float **s0,**s1,dt,dt2,dt3,dt4,dt6,dt12;
	float *k1,*k2,*k3,*k4;
	float diff,diffmax,*scale;
	void *systemptr;

	dim=ode->dim;
	order=ode->order;
	systemptr=ode->systemptr;
	s0=ode->state0;
	s1=ode->state1;
	if(order==1) {																	// *** ORDER 1 ***
		dt=*ode->dtptr;
		for(i=0;i<dim;i++) *s1[i]=*s0[i];
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) *s0[i]=*s1[i]+dt**s0[i]; }
	else if(order==2) {															// *** ORDER 2 ***
		dt=*ode->dtptr;
		k1=ode->k1;
		dt2=dt/2.0;
		for(i=0;i<dim;i++) *s1[i]=*s0[i];
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k1[i]=*s1[i];
			*s1[i]+=dt2**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			*s1[i]=k1[i];
			*s0[i]=k1[i]+dt**s0[i]; }}
	else if(order==4) {															// *** ORDER 4 ***
		dt=*ode->dtptr;
		k1=ode->k1;
		k2=ode->k2;
		dt2=dt/2.0;
		dt3=dt/3.0;
		dt6=dt/6.0;
		for(i=0;i<dim;i++) *s1[i]=*s0[i];
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k1[i]=*s1[i];																// k1[i] is old state
			k2[i]=k1[i]+dt6**s0[i];
			*s1[i]+=dt2**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]+=dt3**s0[i];
			*s1[i]=k1[i]+dt2**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]+=dt3**s0[i];
			*s1[i]=k1[i]+dt**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			*s0[i]=k2[i]+dt6**s0[i];
			*s1[i]=k1[i]; }}
	else if(order==5) {															// *** ORDER 5 ***
		dt=ode->dtsugg>0?ode->dtsugg:*ode->dtptr;
		if(ode->dtmax&&dt>ode->dtmax) dt=ode->dtmax;
		scale=ode->scale;
		k1=ode->k1;
		k2=ode->k2;
		k3=ode->k3;
		k4=ode->k4;
		dt2=dt/2.0;
		dt3=dt/3.0;
		dt4=dt/4.0;
		dt6=dt/6.0;
		dt12=dt/12.0;
		for(i=0;i<dim;i++) *s1[i]=*s0[i];
		if((er=(ode->eqm)(systemptr))) return er;			// *** start of dt step
		for(i=0;i<dim;i++) {
			k1[i]=*s1[i];																// k1 is old state
			k3[i]=*s0[i];
			*s1[i]+=dt2**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]=k1[i]+dt6*k3[i]+dt3**s0[i];
			*s1[i]=k1[i]+dt2**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]+=dt3**s0[i];
			*s1[i]=k1[i]+dt**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++)
			k4[i]=k2[i]+dt6**s0[i];											// k4 is result from dt step

		for(i=0;i<dim;i++)														// start of first dt2 step
			*s1[i]=k1[i]+dt4*k3[i];
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]=k1[i]+dt12*k3[i]+dt6**s0[i];
			*s1[i]=k1[i]+dt4**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]+=dt6**s0[i];
			*s1[i]=k1[i]+dt2**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++)
			*s1[i]=k3[i]=k2[i]+dt12**s0[i];							// k3 is result from dt2 step

		if((er=(ode->eqm)(systemptr))) return er;			// start of second dt2 step
		for(i=0;i<dim;i++) {
			k2[i]=k3[i]+dt12**s0[i];
			*s1[i]+=dt4**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]+=dt6**s0[i];
			*s1[i]=k3[i]+dt4**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			k2[i]+=dt6**s0[i];
			*s1[i]=k3[i]+dt2**s0[i]; }
		if((er=(ode->eqm)(systemptr))) return er;
		for(i=0;i<dim;i++) {
			*s1[i]=k1[i];																// s1 is old state
			*s0[i]=k1[i]=k2[i]+dt12**s0[i]; }						// s0 and k1 are result from second dt2 step

		*ode->dtptr=dt;
		diffmax=0;
		for(i=0;i<dim;i++) {
			diff=fabs(k1[i]-k4[i])/scale[i];
			if(!(diff+1.0>diff)) break;
			if(diff>diffmax) diffmax=diff; }
		if(!(diff+1.0>diff)) {
			ode->dtsugg=dt*0.5;
			for(i=0;i<dim;i++) *s0[i]=*s1[i];
			if(redoctr++==20) return -1;
			return runodestruct(ode); }
		else if(diffmax==0)
			ode->dtsugg=dt*1.1;
		else if(!ode->eps)
			ode->eps=diffmax;
		else if(diffmax<=ode->eps)
			ode->dtsugg=dt*0.95*pow(ode->eps/diffmax,0.25);
		else {
			ode->dtsugg=dt*0.95*pow(ode->eps/diffmax,0.2);
			for(i=0;i<dim;i++) *s0[i]=*s1[i];
			if(redoctr++==20) return -1;
			return runodestruct(ode); }}
		if(ode->dtmax&&ode->dtsugg>ode->dtmax) ode->dtsugg=ode->dtmax;
		redoctr=0;
	return 0; }


/* Example program for 2004 routines */


typedef struct shostate {
	float position[2];
	float velocity[2];
	float omega2;
	float gamma;
	float time; } *shostateptr;

int odestructsho(void *system);


int odestructsho(void *system) {
	shostateptr sys;

	sys=(shostateptr) system;
	sys->position[0]=sys->velocity[1];
	sys->velocity[0]=-sys->omega2*sys->position[1]-2.0*sys->gamma*sys->velocity[1];
	return 0; }


int odestructexample(void) {
	odeptr ode;
	float dt;
	shostateptr sys;
	int er,order;

	order=5;
	dt=0.2;
	sys=(shostateptr)malloc(sizeof(struct shostate));
	if(!sys) return 0;
	sys->position[0]=sys->position[1]=1.0;
	sys->velocity[0]=sys->velocity[1]=-0.1;
	sys->omega2=0.5;
	sys->gamma=0.1;
	sys->time=0;
	ode=allocodestruct(2,order,&dt,(void*)sys,&odestructsho);
	if(!ode) return 0;
	ode->state0[0]=&sys->position[0];
	ode->state1[0]=&sys->position[1];
	ode->state0[1]=&sys->velocity[0];
	ode->state1[1]=&sys->velocity[1];
	if(order==5) ode->dtmax=1.0/sqrt(sys->omega2);	// this keeps amplitude and phase good to at least t=500
	for(;sys->time<500;sys->time+=dt) {
		printf("t,x,v: %f %e %e\n",sys->time,sys->position[0],sys->velocity[0]);
		er=runodestruct(ode);
		if(er) break; }
	freeodestruct(ode);
	return 0; }