/*
 * Routines for filtering the data.
 * filter_data is the top-level routine, which handles all
 * the steps necessary.
 */

#include <stdio.h>
#include <limits.h>
#include <math.h>
#include <stdlib.h>
#include <limits.h>

#include "filter.h"

static int NINT(double t);

static void adjustend (
	double x[],
	double y[],
	int n,
	int cutoff,
	float *a,
	float *b
);

static double part(
	int n,
	double x
); 


#define TWOPI 		6.283185307
#define MAX_PARAM	30

static int numpt, numhm, numpm;

/************************************************************************/
void filter_data (

double 	x[],		/* Input - array containing sample dates */
double 	y[],		/* Input - array containing measurement values */
double 	sig[],		/* Input - array containing uncertainties of measurements */
int 	n0,		/* Input - number of points in x, y, sig */
int 	cutoff1,	/* Input - short term cutoff value in days */
int 	cutoff2,	/* Input - long term cutoff value in days */
float 	interval,	/* Input - regularly spaced sample interval in days */
int 	numpoly,	/* Input - number of polynomial terms in function fit */
int 	numharm,	/* Input - number of harmonic terms in function fit */
float 	pm[],		/* Output - array containing coefficients of function fit */
double 	**covar,	/* Output - array containing covariance matrix */
int 	*numparam,	/* Output - Total number of parameters in pm */
double 	xfilt[],	/* Output - dates of filtered data points */
double 	smooth[],	/* Output - measurement values of filtered data */
double 	trend[],	/* Output - Filter results of residuals using long term cutoff */
double 	deriv[],	/* Output - derivative of trend + polynomial, i.e. growth rate */
int 	*nfilt,		/* Output - number of points in xfilt, smooth, trend, deriv */
float 	*varf1,		/* Output - variance of filter with short term cutoff */
float 	*varf2,		/* Output - variance of filter with long term cutoff */
float 	*chisq,		/* Output - chi squared value of function fit */
float 	*rsd1		/* Output - residual standard deviation of points about the function */
)

{
	float rinterval, cutoff, rmean, ca, cb;
	double dp, p;
	int i, j;
	double *work1, *work2;

	numpt = numpoly;
	numhm = numharm;
	numpm = numpoly + 2*numharm;
	*numparam = numpm;

	work1 = (double *) calloc (n0, sizeof(double));
	work2 = (double *) calloc (MAXSIZE, sizeof(double));


#ifdef DEBUG
printf ("Inside filter_data.  n0 = %d Interval = %f\n", n0, interval);
printf ("Cutoff 1 = %d, Cutoff 2 = %d\n",cutoff1,cutoff2);
printf ("Numpoly = %d, Numharm = %d\n", numpoly,numharm);
printf ("First point = %e,%e\n", x[0], y[0]);
printf ("Last point = %e,%e\n",x[n0-1], y[n0-1]);
#endif

/*
 * Fit the function to the data
 */
	if (numpm > 0) {
		lfit (x, y, sig, n0, pm, covar, numpm, chisq, partial);
	}

#ifdef DEBUG
printf ("Finished lfit\n");
for (i=0; i< numpm; i++) {
  printf ("pm[%d] = %e\n", i, pm[i]);
}
#endif

/*
 *  calculate residuals from fit
 */

	for (i=0; i<n0; i++) {
		work1[i] = y[i] - userfunc (pm, x[i], numhm, numpt);
	}
	means (work1, &rmean, rsd1, n0);

#ifdef DEBUG
printf ( "Finished residuals, rmean = %e, rsd = %e\n", rmean, *rsd1);
#endif

/*
 *  REMOVE A LINEAR TREND BASED ON THE END POINTS OF THE RESIDUALS.
 *  THIS IS TO REDUCE THE END EFFECTS OF THE FILTER
 *
 *
 *  FILTER THE RESIDUALS, FIND VARIANCE DUE TO FILTER
 *
 */

      adjustend (x, work1, n0, cutoff1, &ca, &cb);


#ifdef DEBUG
printf ("Finished adjustend, ca = %e, cb = %e\n", ca,cb);
#endif

	cutoff = (float) cutoff1 / 365.0;
	rinterval = interval / 365.0;
	filter (x, work1, n0, rinterval, cutoff, xfilt, work2, nfilt);

#ifdef DEBUG
printf ("Finished short term filter. nfilt = %d\n", *nfilt);
#endif

	for (i=0; i< *nfilt; i++) {
		smooth[i] = work2[i];
	}

	*varf1 = filtvar (cutoff, rinterval, xfilt, smooth, *nfilt, x, work1, n0);

#ifdef DEBUG
printf ("Finished short term filter variance = %e\n", *varf1);
#endif


/*
 * Add back the linear fit to the ends of the data.
 * Need to add to work1(*) before filtering again.
 */
	for (i=0; i<n0; i++) work1[i] += ca + cb*x[i];
	for (i=0; i< *nfilt; i++) smooth[i] += ca + cb*xfilt[i];

	adjustend (x, work1, n0, cutoff2, &ca, &cb);

#ifdef DEBUG
printf ("Finished adjustend, ca = %e, cb = %e\n", ca,cb);
#endif

	cutoff = (float) cutoff2/365.0;
	filter (x, work1, n0, rinterval, cutoff, xfilt, work2, nfilt);
	for (i=0; i< *nfilt; i++) trend[i] = work2[i];

#ifdef DEBUG
printf ("Finished long term filter\n");
#endif

      *varf2 = filtvar(cutoff, rinterval,xfilt, trend, *nfilt, x, work1, n0);

#ifdef DEBUG
printf ("Finished long term filter variance = %e\n", *varf2);
#endif

	for (i=0; i< *nfilt; i++) trend[i] += ca + cb*xfilt[i];

/*
 *  compute derivative of trend
 */

	spline_f (xfilt, trend, *nfilt, xfilt, work2, deriv, *nfilt);

#ifdef DEBUG
printf ("Finished spline\n");
#endif

/*
 * Find derivative of polynomial at each filtered point
 */

	if (numpoly > 0) {
		for (i=0; i< *nfilt; i++) {
			dp = 0.0;
			p = pm[numpoly-1];
			for (j=numpoly-2; j>=0; j--) {
				dp = dp*xfilt[i] + p;
				p = p*xfilt[i] + pm[j];
			}
			deriv[i] += dp;
		}
	}

	free (work1);
	free (work2);

#ifdef DEBUG
printf ("Finished derivative.  Returning ...\n");
#endif

      return;
}

/************************************************************************/
void spline_f (x, y, n, domain, func, deriv, narg)

double 	x[];		/* Input - array containing x values */
double 	y[];		/* Input - array containing y values */
int 	n;		/* Input - Number of points in x, y */
double 	domain[];	/* Input - where to compute interpolated values along x axis */
double 	func[];		/* Output - the interpolated values */
double 	deriv[];	/* Output - the derivative of the interpolated values */
int 	narg;		/* Input - the number of points in domain, func, deriv */

{
	double *y2;
	int i;

#ifdef DEBUG
printf ("inside spline, n = %d, narg = %d\n", n, narg);
#endif

	y2 = (double *) calloc (n, sizeof(double));

	spline (x, y, n, y2);

	for (i=0; i<narg; i++) {
		splint (x, y, y2, n, domain[i], &func[i], &deriv[i]);
	}

	free (y2);
}


/************************************************************************/
/*  User supplied function defining curve fit
 *  Function is a combination of polynomial terms and
 *  sine, cosine harmonic terms.
 *  f = a1*x + a2*x^2 + X3*x^3 ... for a = 1 to nt  +
 *      p[2*i-1]*sin(2pi*x) + p[2*i]*cos(2pi*x) + ... for i = 1 to nh
 */

double userfunc (pm, x, nh, nt)

float 	pm[];	/* Input - array of parameters from function fit */
double 	x;	/* Input - value at which to calculate the function */
int 	nh;	/* input - number of harmonic terms in p[] */
int	nt;	/* Input - number of polynomial terns in p[] */

{
	double v, p;

	v = yearharmonic (pm, x, nt, nh);
	p = polyv(pm, x, nt);

	return (v+p);
}

/************************************************************************/
double polyv (pm, x, numpt)
float pm[];	/* Input - array of parameters from function fit */
double	x;	/* Input - Value at which to calculate polynomial */
int 	numpt;	/* Input - Number of polynomial terms in pm[] */

{
	double p;
	int j;

	if (numpt <= 0) {
		return (0.0);
	}

	p = pm[numpt-1];
	for (j=numpt-2; j >= 0; j--) {
		p = p*x + pm[j];
	}

	return (p);
}

/************************************************************************/
double yearharmonic (pm, x, numpt, numh)
float pm[];	/* Input - array of parameters from function fit */
double	x;	/* Input - Value at which to calculate polynomial */
int 	numpt;	/* Input - Number of polynomial terms in pm[] */
int 	numh;	/* Input - Number of harmonic terms in pm[] */

{
	double s, twopix;
	int i, ix;

	s = 0.0;
	twopix = TWOPI*x;
	for (i=0; i<numh; i++) {
		ix = 2*i + numpt;
		s += pm[ix]*sin((i+1)*twopix) + pm[ix+1]*cos(twopix*(i+1));
	}

	return (s);
}

/************************************************************************/
void means (array, rmean, sdev, n)

double 	array[];/* Input - array of values to calculate mean */
float  	*rmean;	/* Output - mean value */
float	*sdev;	/* Output - standard deviation */
int	n;	/* Input - number of values in a[] */

{
	double sum, sumsq;
	int i;
	
	*rmean = 0.0;
	*sdev = 0.0;
	if (n < 1) {
		fprintf (stderr, "Warning: n < 1 in routine means.\n");
		return;
	}

	sum = 0.0;
	sumsq = 0.0;
	for (i=0; i<n; i++) {
		sum += array[i];
		sumsq += (array[i]*array[i]);
	}

	*rmean = sum/((float) n);
	if (n > 1) {
		*sdev = sqrt ((sumsq - (sum*sum)/(float) n) / ((float) (n-1)));
	}
	return;
}


/************************************************************************/
double varnce (covar, n, x)

double 	**covar;	/* Input - array of covariances of parameters found by 'lfit' */
int 	n;		/* Input - number of parameters */
double 	x;		/* Input - time value at which to calculate variance */

{
	double s, s2, p, p2;
	int i, j;
	
	s = 0.0;
	s2 = 0.0;
	if (n == 0) return (0.0);

	for (i=0; i<n-2; i++) {
		p = part(i,x);
		s += covar[i][i] * p * p;
		for (j = i+1; j<n; j++) {
			p2 = part(j,x);
			s2 += covar[i][j] * p * p2;
		}
	}
	p = part(n-1, x);
	s += covar[n-1][n-1] * p * p;
	s += 2.0*s2;

	return (s);

}


/************************************************************************/
static void adjustend (x, y, n, cutoff, a, b)
double x[];
double y[];
int n;
int cutoff;
float *a;
float *b;
{
	double *xp, *yp;
	int k, i;
	float c, siga, sigb, chi2;
	

	xp = (double *) calloc (n, sizeof(double));
	yp = (double *) calloc (n, sizeof(double));

	*a = 0.0;
	*b = 0.0;
	if ((x[n-1] - x[0]) < (float) (cutoff) / 365.0) return;

	c = (float) cutoff /365.0 / 4.0;
	k = 0;
	for (i=0; i<n; i++) {
		if ((x[i] <= x[0]+c) || (x[i] >= x[n-1]-c)) {
			xp[k] = x[i];
			yp[k] = y[i];
			k++;
		}
	}

	fit (xp, yp, k, a, b, &siga, &sigb, &chi2);

	for (i=0; i<n; i++) {
		y[i] -= (*a + *b*x[i]);
	}

	free(xp);
	free(yp);

	return;
}

/*********************************************************************/
/*
 * Calculate the partial derivatives of user function
 * with respect to each parameter
 *
 * x   - Value at which to find partial derivative
 * der - Array of partial derivatives at time x
 * numpm - Number of parameters in der
 *
 * Calls routine 'part(i,n)' which returns the value of the 
 * partial derivative with respect to parameter i at time x.
 * Written this way since the routine varnce needs to use 'part' also.
 *
 */
/*********************************************************************/
void partial (x, der, numpm)
double x;
double der[];
int numpm;
{
	int i;

	for (i=0; i<numpm; i++) {
		der[i] = part(i,x);
	}
}

/*********************************************************************/
/*
 * Find partial derivative of user function for parameter n at x
 * with respect to coefficients.
 *
 * n - Number of the parameter 0..numpm-1
 * x - Value at which to calculate the partial derivative
 * numpt - number of polynomial terms in user function
 *
 */
/*********************************************************************/
static double part(n,x)
int n;
double x;
{
	double p = 0.0, xx;
	int ix;

	if (n < numpt) {
		if ( n==0) {
			p = 1.0;
		} else {
			p = pow (x, (double) n);
		}
	} else {
		ix = ((n-numpt) /2 + 1);
		xx = ix * TWOPI * x;
		if ((n-numpt) % 2 == 0) {
			p = sin(xx);
		} else {
			p = cos(xx);
		}
	}
	return (p);
}

/*****************************************************/
void filter (
double x[],
double y[],
int n,
float dinterv,
float cutoff,
double x1[],
double filt[],
int *np
)
{
	int i, j;
	int n2, numzer, inzhalf, n3;
	double h, w;
	float b, dmaxfreq, freq, cutoff2, rw;
	double pn;
	

#ifdef DEBUG
printf("Inside filter\n");
printf ("dinterv = %f, n = %d\n",dinterv,n);
printf ("x[n-1] = %e, x[0] = %e\n", x[n-1], x[0]);
printf ("y[n-1] = %e, y[0] = %e\n", y[n-1], y[0]);
#endif

/* Determine how many points there will be from first data point
 * to last data point using equally spaced intervals.
 */

	pn = (x[n-1] - x[0])/dinterv;
	*np = NINT(pn) + 1;
#ifdef DEBUG
printf ("np = %d\n", *np);
#endif

/* Determine the number of points >= np that is a power of 2 */

	n2 = 1;
	while (*np > n2) {
		n2 = n2<<1;
	}

	if (n2 > MAX_OBS) {
		fprintf (stderr, "Error:  Too many data points to filter.\n");
		fprintf (stderr, "Maybe try using larger interval size.\n");
		exit(1);
	}

/*
 * 
 *  zero pad to get 2**p points
 *  n3 - number of zeros at front end of data
 *       in order to remember where real data starts in filt[*]
 */

	numzer = n2 - *np;
	inzhalf = numzer/2;
	for (i=0; i<n2+2; i++) filt[i] = 0.0;
	n3 = inzhalf;
	
#ifdef DEBUG
printf ("n2 = %d, numzer = %d, n3 = %d\n", n2, numzer, n3);
#endif

/*
 * compute equally spaced values using linear interpolation
 */

	i=0;
	x1[0] = x[0];
	filt[n3] = y[0];
	for (j=1; j< *np; j++) {
		x1[j] = x[0] + j*dinterv;
		/* Find x value greater than x1[j] */
		/* Complicated by the fact that the
		   x values could be pairs, meaning the last
		   2 points have the same x value */
		while (x1[j] >= x[i]) {
			i++;
			if (i >= (n-1)) break;
                }

		i--;

		/* If we are past the last actual data point, 
		   and the last two points were a pair, then
		   the x data points will be the same,
		   making w infinite. So instead of extrapolating,
		   just set the point to 0 (which means assume
		   it is equal to the function value at that time). */
		if (x1[j] > x[n-1]) { 
			filt[j+n3] = 0.0; 
		} else {
			h = x1[j] - x[i];
			w = (y[i+1]-y[i])/(x[i+1]-x[i]);
			filt[j+n3] = w*h + y[i];
		}

	}

/*
 * Transform the data (time >> frequency)
 */

	realft (filt-1, n2, 1);

#ifdef DEBUG
printf ("Finished forward fft\n");
#endif

/*
 * filter the data by multiplying the frequency domain data
 * with the response of a low pass filter.
 * b - frequency step size
 * dmaxfreq - nyquist frequency
 */

	b = 1.0/(n2*dinterv);
	dmaxfreq = 1.0/(2*dinterv);
	cutoff2 = 1.0/cutoff;
	for (i=2; i<n2+2; i+=2) {
		freq = (float) i/2.0 * b;
		rw = fnfilt(freq, cutoff2, 6);
		filt[i] *= rw;
		filt[i+1] *= rw;
	}
	rw = fnfilt (dmaxfreq, cutoff2, 6);
	filt[1] *= rw;

#ifdef DEBUG
printf ("Finished lowpass filter response\n");
#endif

/*
 * Inverse transform the data (frequency >> time)
 */
	realft (filt-1, n2, -1);

#ifdef DEBUG
printf ("Finished inverse fft\n");
#endif

/*
 * remove zero padding
 */

	for (i=0; i< *np; i++) {
		filt[i] = filt[n3+i];
	}

#ifdef DEBUG
printf ("Finished removing zero padding\n");
#endif

	return;
}

/**********************************************************************/
float fnfilt (float freq, float sigma, int power)
{
	float z, f;

/* Watch out for underflow */

	z = pow ((double) freq/sigma, (double) power);
	if (z > 20.0) {
		f = 1e-10;
	} else {
		f = 1.0/(pow (2.0, (double) z));
	}
	return (f);
}
/*****************************************/
static int NINT(double t)
{
	if (t<0.0) {
		return ((int) t-.5);
	} else {
		if ((t+.5) > (double) (INT_MAX))
			return ((INT_MAX));
		else
			return ((int) (t+.5));
	}
}