//
//  DDErk23.m
//  ddetest
//
//  Created by Ashley on 25/03/2007.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "DDErk23.h"

@implementation DDErk23

- (id) initWithRhs:(id <rhs_protocol, NSObject>)rhs
{
	self = [super init];
	
	if (self!=nil) {
		[rhs retain];
		_rhs = rhs;
		int nState = [rhs nVariables];
		_k2 = (double *)calloc(nState, sizeof(double));
		_k3 = (double *)calloc(nState, sizeof(double));
		_k4 = (double *)calloc(nState, sizeof(double));
	}
	
	return self;
}

- (id <rhs_protocol, NSObject>) rhs
{
	return _rhs;
}

//void rk23(state,newstate,g,newg,error,coeff,ns,time,dt)
//double *state,*newstate,*g,*newg,*error,*coeff,time,dt;int ns;
- (void) step:(double *)state newState:(double *)newState gradients:(double *)gradients newGradients:(double *)newGradients
			  error:(double *)error time:(double)t timeStep:(double)dt

/* Takes a single integration step from time to time+dt using a 3rd order
   embedded Runge-Kutta Fehlberg method:
   E.Hairer, S.P.Norsett & G.Wanner 1987, Solving Ordinary differential
   Equations I. springer-Verlag Berlin. p170 RKF2(3)B
   The routine returns an estimated error vector for adaptive timestepping.
   The gradient of the state variables is to be given in function grad().
   The routine uses the lower order scheme for updating,
   fortunately Fehlberg optimised the coefficients for the lower order
   scheme..... 4/10/95.
   NOTE: not yet optimised for minimum gradient evaluations - see original
   table of coeffs. Partially optimised 9/10/95 Only valid for ci=b4i!
   Takes gradient at time in g, puts gradient at time+dt in newg - these can
   be the same pointer/array */


{ // static int first=1,oldns=-1;
//  static double *k1,*k2,*k3,*k4,
	
//	Embedded RKF table - coded this way to save addressing time.
	double	a2=0.25,  a3=27.0/40.0,
			b21= 0.25,
			b31=-189.0/800.0,  b32= 729.0/800.0,
			b41= 214.0/891.0,  b42= 1.0/33.0,     b43= 650.0/891.0,
			cc1= 533.0/2106.0, cc3= 800.0/1053.0, cc4=-1.0/78.0;
  
	int i;
/*  if ((first)||(oldns!=ns))
  { if (!first)
    { free(k2);free(k3);free(k4);}
    oldns=ns;first=0;
    if (ns>0)
    { 
    }
  } */
	double *k1 = gradients;
  
	id rhs = [self rhs];
	int ns = [rhs nVariables];
  
	for (i=0; i<ns; i++) {
		newState[i] = state[i] + (k1[i]*b21)*dt;
	}
	[rhs gradients:_k2 forState:newState atTime:(t + dt*a2)];
  
	for (i=0; i<ns; i++) {
		newState[i] = state[i] + (k1[i]*b31 + _k2[i]*b32)*dt;
	}
  
	[rhs gradients:_k3 forState:newState atTime:(t + dt*a3)];
	for (i=0; i<ns; i++) {
		newState[i] = state[i] + (k1[i]*b41 + _k2[i]*b42 + _k3[i]*b43)*dt;
	}

	[rhs gradients:_k4 forState:newState atTime:(t + dt)];
	for (i=0; i<ns; i++) { 
		newGradients[i] = _k4[i];
		error[i] = state[i] + (cc1*k1[i] + cc3*_k3[i] + cc4*_k4[i])*dt - newState[i];
	}
}

- (void) dealloc
{
	[_rhs release];
	free(_k2);
	free(_k3);
	free(_k4);
	[super dealloc];
}

@end
