/*
**  $Id: expdelay.c,v 1.2 1998/02/04 21:18:48 frank Exp $
**  expdelay.c 1.0	Solars 2.6 Unix	971218	SIO/ODF	fmd
**
**  Exponential delay filter functions.
**
**  This filter implements a simple exponential response with
**  an optional delay.
*/
#include <stdlib.h>
#include <odflib.h>
#include <math.h>

#ifdef FORTRAN
/*
      subroutine tlag(temp,timlag,sample,freq,missed,work,tlaged)
c
c     title:
c     *****
c
c       tlag  -- single exponential delay filter
c
c     purpose:
c     *******
c
c       to delay a time-series, providing an exponential response
c
c     parameters:
c     **********
c
c        temp   -> time-series value to be lagged
c        timlag -> time lag in seconds
c        sample -> sample every 'sample' point
c        freq   -> frequency of discrete sample in points per second
c        missed -> number of missing points since last point
c        work   <> 5 element work area
c        tlaged <- lagged value
c
         real temp,timlag,freq,missed,tlaged
         real sample,work(5)
*/

void
tlag_(float *temp, float *timlag, float *sample, float *freq,
	 float *missed, float *work, float *tlaged)
{
	/* 
	** work:
	** parameter(alpha=1,tsum=2,tlast=3,samcnt=4,totmis=5)
	*/
	ExpDelay	pE;
	ExpDelayDesc	e;

	if (*missed < 0.0) {
	    pE		= ExpDelayInit(0, *timlag, 1.0/(*freq),
				*temp);
	    work[0]	= pE->alpha;
	    work[1]	= pE->lagValue;
	    work[2]	= pE->prevValue;
	    free(pE);
	    *tlaged	= *temp;
	} else {
	    memset(&e, 0, sizeof (ExpDelayDesc));
	    e.alpha	= work[0];
	    e.lagValue	= work[1];
	    e.prevValue	= work[2];
	    *tlaged	= (float)ExpDelayCalc(&e, (int)(*missed), *temp);
	    work[1]	= e.lagValue;
	    work[2]	= e.prevValue;
	}
}

#else FORTRAN

ExpDelay
ExpDelayInit(double delayPeriod, double lagPeriod, double sampPeriod,
		double initVal)
{
	int		i, qLen, *pM;
	double		*pQ;
	ExpDelay	pE;

	qLen		= ceil(delayPeriod/sampPeriod);
	if (qLen < 0) qLen=0;
	pE		= (ExpDelay)malloc(sizeof (ExpDelayDesc)+
				qLen * (sizeof (double)+sizeof (int)));
	if (qLen > 0) {
	    pE->q	= (double *)&pE[1];
	    pE->miss	= (int *)&pE->q[qLen];
	    for (i=0, pQ=pE->q, pM=pE->miss; i<qLen; *pQ++ = initVal,
		*pM++ = 0, i++);
	} else
	    pE->q	= NULL;
	pE->qLen	= qLen;
	pE->alpha	= exp(-sampPeriod/lagPeriod);
	pE->prevValue	=
	pE->lagValue	= initVal;
	pE->last	= 0;
	pE->qLen	= qLen;
	return (pE);
}

double
ExpDelayReset(ExpDelay pE, double initVal)
{
	int		i, qLen, *pM;
	double		*pQ;
	if ((qLen=pE->qLen))
	    for (i=0, pQ=pE->q, pM=pE->miss; i<qLen; *pQ++ = initVal,
		*pM++ = 0, i++);
	pE->lagValue	=
	pE->prevValue	= initVal;
	pE->last	= 0;
	return (initVal);
}

double
ExpDelayCalc(ExpDelay pE, int missing, double val)
{
	double		newVal, prevVal, meanVal, w, atemp, *pQ;
	double		alpha = pE->alpha, lagVal;
	int		*pM, cnt, index, missed, qLen = pE->qLen;
    /*
    **  Apply optional delay (dequeue a value).
    */
	if (qLen) {
	    pQ		= pE->q;
	    pM		= pE->miss;
	    index	= pE->last;
	    prevVal	= pQ[index];
	    missed	= pM[index];
	} else {
	    prevVal	= val;
	    missed	= missing;
	}
    /*
    **  Interpolate lag if missing values.
    */
	if (missed > 0) {
	    atemp	= 0.0;
	    w		= 1.0;
	    for (cnt=1; cnt<=missed; cnt++) {
		w	*= alpha;
		atemp	+= w;
	    }
	    w		*= alpha;
	    meanVal	= (pE->prevValue+prevVal)*0.5;
	    lagVal	= w*pE->lagValue +
			  (1.0-alpha)*(prevVal + atemp*meanVal);
	} else
	    lagVal	= pE->lagValue;
    /*
    **  Calculate lagged value.
    */
	pE->lagValue	= 
	newVal		= alpha * (lagVal - prevVal) + prevVal;
	pE->prevValue	= prevVal;
    /*
    **  Delay (enqueue) new value.
    */
	if (qLen) {
	    pQ[index]	= val;
	    pM[index]	= missing;
	    pE->last	= (++index) % qLen;
	}
	return (newVal);
}
#endif FORTRAN