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

#import "DDEHistory.h"

@implementation DDEHistory

- (id) initWithRhs:(id)rhs
{
	[super init];
	
	int nhv = [rhs nHistory];
	long histsize = [rhs histSize];
	int nlag = [rhs nLag];
	
	_no = (long)nhv;
	_size = histsize;
	int i;
	_lagmarker = (long **)calloc((size_t)nhv, sizeof(long *));
	for (i=0; i<nhv; i++) {
		_lagmarker[i] = (long *)calloc((size_t)nlag, sizeof(long));
	}
	
	_clock = (double *)calloc((size_t)_size, sizeof(double));
	
	_buff = (double **)calloc((size_t)nhv, sizeof(double *));
	for (i=0; i<nhv; i++) {
		_buff[i] = (double *)calloc((size_t)_size, sizeof(double));
	}
	
	_gbuff = (double **)calloc((size_t)nhv, sizeof(double *));
	for (i=0; i<nhv; i++) {
		_gbuff[i] = (double *)calloc((size_t)_size, sizeof(double));
	}
	if (!_gbuff[nhv-1]) { 
		printf("History buffer too long\n");
		return nil;
	}
	
	_his = (double *)calloc((size_t)_no, sizeof(double));
    _ghis = (double *)calloc((size_t)_no, sizeof(double));
	
	_offset = -1L;
	_first = YES;
	
	_rhs = [rhs retain];
	
	return self;
}

- (void) dealloc
{
	int nhv = [_rhs nHistory];
	int i;
	for (i=0; i<nhv; i++) {
		free(_lagmarker[i]);
		free(_buff[i]);
		free(_gbuff[i]);
	}
	free(_lagmarker);
	free(_buff);
	free(_gbuff);
	free(_clock);
	free(_his);
	free(_ghis);
	[_rhs release];
	[super dealloc];
}

- (void) updateHistory:(double *)gradients state:(double *)state time:(double) t
{
	if (_first) {
		_firstTime = t;
		_first = NO;
	}
	
	id rhs = [self rhs];

	[rhs storeHistory:_his gradientsHistory:_ghis gradients:gradients state:state time:t];
	_lastTime = t;
	_offset++;
	if (_offset==_size) {
		_offset = 0L;
	}
	_clock[_offset] = t;
	
	int i;
	for (i=0; i<_no; i++) {
		_buff[i][_offset] = _his[i];
		_gbuff[i][_offset] = _ghis[i];
	}

}

- (double) pastValue:(int)i time:(double)t mark:(int)markno
{
	long k1;
	long k;
	long offset;
	long offsetplus;
	long size;
	double res;
	double *y;
	double *g;
	double *x;
	double x0;
	double x1;
  
//  for (i=0;i<data.no_c;i++) printf("%4d %15.6lf\n", i, data.c[i]); printf("\n");
	y =_buff[i];
	g = _gbuff[i];
	x = _clock; 
	offset = _offset;
	size = _size;
	if (x[offset]==t) {
		return(y[offset]);
	}
  
	offsetplus = offset + 1L; 
	if (offsetplus==size) {
		offsetplus = 0L;
	}
	k = _lagmarker[i][markno];
	k1 = k + 1L;
	if ((k1>=size)||(k1<0L)) {
		k1=0L;
	}
  
	while ((x[k1]<t) && (k1!=offset)) {
		k1++;
		if (k1==size) {
		k1=0L;
		}
	}
	if (k1==0L) {
		k=size-1L; 
	} else {
		k=k1-1L;
	}

	while ((x[k]>t) && (k!=offsetplus)) { 
		if (k==0L) {
			k=size-1L; 
		} else {
			k--;
		}
	}
	
	k1 = k + 1L;
	if (k1==size) {
		k1=0L;
	}
	if (t<x[k]) {
		printf("Lag too large for history buffer\n");
		exit(0);
	}
  
	x0 = x[k];
	x1 = x[k1];
	if ((t>x[k1])&&(x[k]==x[k1])) {
		res=y[k1]+(t-x[k1])*g[k1]; 
	} else { 
//		        HERMITE(res,x0,x1,y[k],y[k1],g[k],g[k1],t);
//		#define HERMITE(res,x0,x1,y0,  y1,   g0,   g1,   x)
        double xx0 = t - x0;
		double xx1 = t - x1;
		double xx12 = xx1*xx1;
        double xx02 = xx0*xx0;
		double h = x1 - x0;
        if (h!=0.0) {
			res = ((g[k]*xx0*xx12 + g[k1]*xx1*xx02 +
				   (y[k]*(2.0*xx0 + h)*xx12 - y[k1]*(2.0*xx1-h)*xx02)/h)/(h*h)); 
		} else  {
			res = y[k];
		}
	}
//  if (x[k1]==x[k]) return(y[k1]);
  /*printf("\nx0=%g   x1=%g  yk=%g  yk1=%g\n  gk=%g  gk1=%g  res=%g",
  x0,x1,y[k],y[k1],g[k],g[k1],res); */
	_lagmarker[i][markno] = k;
	return res;
}

- (void) setRhs:(id)rhs
{
	[rhs retain];
	[_rhs release];
	_rhs = rhs;
}

- (id) rhs
{
	return _rhs;
}

@end
