//
//  DDESolver.m
//  ddetest
//
//  Created by Ashley on 26/03/2007.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "DDESolver.h"
#import "DDEIStepper.h"

#define ERRCON 1.89e-4

@implementation DDESolver

- (id) initWithRhs:(id)rhs stepper:(id)stepper
{
	self = [super init];
	
	if (self!=nil) {
		_stepper = [stepper retain];
		if ([rhs nHistory]>0) {
			_history = [[DDEHistory alloc] initWithRhs:rhs];
		} else {
			_history = nil;
		}
		[rhs setHistory:_history];
		_rhs = [rhs retain];
		
	}
	
	return self;
}

- (id) rhs
{
	return _rhs;
}

- (id) history
{
	return _history;
}

- (id) stepper
{
	return _stepper;
}

- (void) solve
{
	int i;
	long iout = 1L;
	double *newsws;
	double *sws;
	double *news;
	double *newg;
	double *err;
	double *e0;
	double *scale;
	double *g;
	
	id rhs = [self rhs];
	int nsw = [rhs nSwitches];
	int ns = [rhs nVariables];

	double *state = (double *)calloc(ns, sizeof(double));
	[rhs initState:state];

	double t0 = [rhs t0];
	double t1 = [rhs t1];
	double dt = [rhs dt];
	double eps = [rhs eps];
	double dout = [rhs outputStep];
	
	newsws = (double *)calloc(nsw, sizeof(double));
	sws = (double *)calloc(nsw, sizeof(double));
	news = (double *)calloc(ns, sizeof(double));
	newg = (double *)calloc(ns, sizeof(double));
	err = (double *)calloc(ns, sizeof(double));
	e0 = (double *)calloc(ns, sizeof(double));
	scale = (double *)calloc((size_t)ns, sizeof(double));
/*		statescale(_scale); */
	[rhs stateScale:scale];
	if (nsw>0) {
		[rhs switchFunctions:sws newState:state time:t0];
	} 

	g = (double *)calloc(ns, sizeof(double));
		
//	double mindt;
//	double maxdt;
	int accepted = 0;
	int rejected = 0;

//	id rhs = [self rhs];
//	int ns = [rhs nVariables];

	double ti = t0;
	double t = t0;
	double *sp = state;
	double D = dt;
	double mindt = D*1e-9;
	double maxdt = D*100.0;
	
	double *nswp = newsws;
	double *swp = sws;
	double *nsp = news;
	
	int fixstep = 0;
	
	int nhv = [rhs nHistory];
//	int hbsize = [rhs histSize];
//	int nlag = [rhs nLag];
//		inithisbuff(nhv, hbsize, nlag);

//		grad(g,s,c,t0, pastvalue);
	[rhs gradients:g forState:state atTime:t0];
//		updatehistory(g,s,c,t0);
	DDEHistory *history = nil;
	if (nhv>0) {
		history = [self history];
		[history updateHistory:g state:state time:t0];
	}
	id stepper = [self stepper];

//	if (dout) output(s, t0, data);
	if (dout>0.0) {
		[rhs output:state time:t];
	}
	
	while (t0<t1) { 
//		printf("%8.3f %8.3f %12.6g\n", t, t0, state[0]);
		double target;
		if (t0+D>t1) { 
			target = t1;
		} else { 
			target = t + D;
		}
//		t = istep(sws,newsws,s,news,g,newg,c,err,t0,target,nsw,ns,&swi);
		int swi = -1;
//		t = [self iStep:_sws newSwitches:_newsws state:state newState:_news gradients:_g newGradients:_newg error:_err 
//				  startTime:t0 stopTime:target /*nsw,ns,*/ switchCounter:&swi];
//		[stepper step:state newState:news gradients:g newGradients:newg error:err time:t timeStep:D];
//		t = target;
		t = [stepper istep:sws newSwitches:newsws state:state newState:news gradients:g newGradients:newg error:err 
				  startTime:t0 stopTime:target flickedSwitch:&swi];
		double errmax = 0.0;
		if ((!fixstep) && (D>mindt)) {
			for (i=0;i<ns;i++) {
				e0[i] = eps*(fabs(state[i]) + fabs((t - t0)*(g[i] + newg[i])*0.5) + scale[i]);
			}
			for (i=0; i<ns; i++) { 
				double rerr;
				if ((err[i]<1e-150) && (err[i]>-1e-150)) {
					rerr = 0.0;
				} else {
					rerr = err[i]/e0[i];
				}
				rerr = fabs(rerr);
				if (rerr>errmax) {
					errmax = rerr;
				}
			}
		}

		if (errmax<1.0) { 
//			printf("Step accepted\n");
			accepted++;
			double *dum;
			dum=state;state=news;news=dum;
			dum=sws;sws=newsws;newsws=dum;
			dum=g;g=newg;newg=dum;
//			updatehistory(g,s,c,t);
			[history updateHistory:g state:state time:t];
/*			if (dout>0.0) {
				if ( t > ti+iout*dout ) { 
					output(s, t, data);
					while (ti+iout*dout<=t) iout++;
				}
			} */
			if (dout>0.0) {
				if (t > ti + iout*dout ) { 
					[rhs output:state time:t];
					while (ti + iout*dout<=t) {
						iout++;
					}
				}
			} 
			t0 = t;
			if ( swi > -1 ) { 
				if (dout>0.0) {
					[rhs output:state time:t];
				}
//				map(s,c,t,swi);
				[rhs map:state time:t switch:swi];
				if (dout>0.0) { 
					[rhs output:state time:t];
				} 
//				grad(g,s,c,t, pastvalue);
//				updatehistory(g,s,c,t);
//				[rhs map:state time:t switch:swi];
				[rhs gradients:g forState:state atTime:t];
				[history updateHistory:g state:state time:t];
			} else { 
				if ((!fixstep)&&(t<t1)) {
					D = (errmax > ERRCON) ? 0.95*D*pow(errmax,-0.2) : 5.0*D;
				}
				if (D>maxdt) D = maxdt;
			}
		} else { 
//			printf("Step rejected %g %g %g\n", errmax, t, t0);
/* Step actually achieved */
			double Da = t - t0; 
			rejected++;
/* Shrink D from Da */
			D = 0.95*Da*pow(errmax, -0.25);
			D = (D < 0.1*Da) ? 0.1*Da : D ;
			if (D < 1e-14*(dt)) {
				printf("Step size failure\n");
			}
			t = t0;
		}
		(dt) = D;
	}
	for (i=0;i<ns;i++) {
		sp[i]=state[i]; /* copying results to correct address */
	}
	free(swp);
	free(nswp);
	free(nsp);
	free(err);
	free(e0);
	free(newg);
	free(scale);
//	free(state);
}

- (void) dealloc
{
	[_stepper release];
	[_rhs release];
	[_history setRhs:nil];
	[_history release];
	[super dealloc];
}

@end
