//
//  DDEIStepper.m
//  ddetest
//
//  Created by Ashley on 09/04/2007.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "DDEIStepper.h"

void error(char *str)
{
	printf("%s\n", str);
	exit(-1);
}

double zeropos(double x1, double x2, double x3, double s1, double s2, double s3)
//double x1,x2,x3,s1,s2,s3;
/* finds the root in [x1,x3] of a quadratic passing through the (xi,si)s
   it is assumed that s3<s1*/
{ 
	double z,y,zpy,a,b,c,d,a1,b1,c1,p;
	int ok=1;
//	static int first=1;
	double udge = 1.00000001;
/*	if (first) { 
		first=0;
		udge=1.00000001;
	} */
	
	z = x3 - x2;
	y = x2 - x1;
	zpy = z + y;
	if (z==0.0 || y==0.0) {
		error("Error in switching: zero switch interval");
	}
    a1 = a= s2;
    c1 = c = (z*s1 + y*s3 - zpy*s2)/(zpy*z*y);
    b1 = b = (s2 - s1)/y + c*y;
    d = b*b - 4.0*a*c;
	c *= 2.0;
/* linear only approximation - in case c numerically zero */
	p = -a/b; 
	if (c==0.0) {
		a = p;
	} else { 
		if (d>=0.0) { 
			d = sqrt(d);
			a = (-b + d)/c;
			b = (-b - d)/c;
			if ((b>=-y)&&(b<=z)) {
				a = b; 
			} else {
				if ((a<-y) || (a>z)) {
					ok = 0;
				}
			}
		}
		if ((d<0.0) || (!ok)) { 
			if (-s3<s1) a = z; else a = -y;
		}
		z = a1 + a*b1 + a*a*c1;
		d = a1 + p*b1 + p*p*c1;
// check that linear interpolation is not better 
		if (fabs(z)>fabs(d)) {
			a = p; 
		}
	}
	a += x2;
	if (a>x3) {
		a = x3;
	}
	if (a<=x1) { 
		if (a==0.0) {
			a = udge - 1.0; 
		} else if (a<0.0) {
			a /= udge; 
		} else {
			a *= udge;
		}
	}
	
	return a;
}

@implementation DDEIStepper

- (id) initWithRhs:(id /*<rhs_protocol, NSObject>*/) rhs
{
	self = [super initWithRhs:rhs];
	int nsw = [rhs nSwitches];
	int ns = [rhs nVariables];
	
	_sw1 = (double *)calloc(nsw, sizeof(double));
    _sw2 = (double *)calloc(nsw, sizeof(double));
    _s1 = (double *)calloc(ns, sizeof(double));
    _s2 = (double *)calloc(ns, sizeof(double));
    _err1 = (double *)calloc(ns, sizeof(double));
    _flicked = (int *)calloc(nsw, sizeof(int));
//	_rhs = [rhs retain];
	
	return self;

}

- (double) istep:(double *)sw0 newSwitches:(double *)newsws state:(double *)s0 newState:(double *)news 
				gradients:(double*)g newGradients:(double *)newg error:(double *)err 
				startTime:(double)t0 stopTime:(double)t1 flickedSwitch:(int *)flickedswitch
{
	id rhs = [self rhs];
	int nsw = [rhs nSwitches];
	int ns = [rhs nVariables];
	
	double dt = t1-t0;
//	rk23(s0,news,g,newg,err,c,ns,t0,dt);
	[self step:s0 newState:news gradients:g newGradients:newg error:err time:t0 timeStep:dt];
	if (nsw!=0) {
		[rhs switchFunctions:newsws newState:news time:t1];
	}

// Are there any switches?
	int i;
	int switches=0;
	for (i=0;i<nsw;i++) {
		if ((sw0[i]>0.0) && (newsws[i]<=0.0)) { 
			_flicked[switches] = i;
			switches++;
		}
	}
// If no switches then its an ordinary step 
	if (!switches) { 
		*flickedswitch = -1;
		return t1;
	}

// Logic for stepping to first switch 
	double sp1 = t0 + dt*0.5;
// if k gets to 100 routine fails
	int k;
	for (k=0;k<100;k++) { 
// step to approx. 1st switch position
//		rk23(s0,s1,g,newg,err,c,ns,t0,sp1-t0); 
		[self step:s0 newState:_s1 gradients:g newGradients:newg error:err time:t0 timeStep:sp1 - t0];
		[rhs switchFunctions:_sw1 newState:_s1 time:sp1];

		switches = 0;
//are there any switches ? 
		for (i=0;i<nsw;i++) {  
			if ((sw0[i]>0.0) && (_sw1[i]<=0.0)) { 
				_flicked[switches] = i;
				switches++;
			}
		}

		if ((k) && (switches==1)) { 
			*flickedswitch = _flicked[0];
			for (i=0;i<ns;i++) {
				news[i] = _s1[i];
			}
			for (i=0;i<nsw;i++) {
				newsws[i] = _sw1[i];
			}
			return(sp1);
		}

// step to end of interval 
//    rk23(s1,s2,newg,newg,err1,c,ns,sp1,t1-sp1);
		[self step:_s1 newState:_s2 gradients:newg newGradients:newg error:_err1 time:sp1 timeStep:t1 - sp1];
//    switchfunctions(sw2,s2,c,t1);
		[rhs switchFunctions:_sw2 newState:_s2 time:t1];

// are there any switches?
		for (i=0;i<nsw;i++) {
			if ((_sw1[i]>0.0) && (_sw2[i]<=0.0)) { 
				_flicked[switches] = i;
				switches++;
			}
		}
		
		if (!switches) { 
			*flickedswitch=-1;
			for (i=0;i<ns;i++) { 
				news[i] = _s2[i]; 
				err[i] = sqrt(err[i]*err[i] + _err1[i]*_err1[i]);
			}
			for (i=0;i<nsw;i++) {
				newsws[i] = _sw2[i];
			}
			return(t1);
		}

// having got this far switch positions must be estimated 

// locate the first switch 
		double sp2 = t1;
		double minp=t1;
		for (i=0;i<switches;i++) { 
			double zp;
			if ((t0==t1)||(sp1==t1)||(t0==sp1)) {
				zp = t1; 
			} else {
				zp = zeropos(t0, sp1, t1, sw0[_flicked[i]], _sw1[_flicked[i]], _sw2[_flicked[i]]);
			}
			if (zp<minp) { 
				sp2 = minp;
				minp = zp;
			}
		}
		sp1 = minp;
		double udge = 1e-9;
		double ds = sp2 - sp1;
		if (ds>0.0) {
			do { 
				sp1+=udge*ds;
				udge*=10.0;
			}  while (sp1==minp);
		}
	}
	error("Problem with switch logic");
	return(t0);
}

- (void) dealloc
{
	free(_flicked);
	free(_err1);
	free(_s1);
	free(_s2);
	free(_sw1);
	free(_sw2);
//	[_rhs release];
	[super dealloc];
}

@end
