/* (C) Copr. 1986-92 Numerical Recipes Software ##'. */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "filter.h"
#include "dates.h"


static double swap;
#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}

static void covsrt( double **covar, int ma);

static float sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)

void nrerror(char error_text[]);
void gaussj(double **a,int n,double **b,int m);

/***************************************************************/
/*
 * Apply a linear fit to data contained in x and y arrays.
 * Return the intercept, slope, sigmas of both, and chi-squared.
*/
/***************************************************************/
void fit(
	double x[],
	double y[],
	int ndata,
	float *a,
	float *b,
	float *siga,
	float *sigb,
	float *chi2
)

{
        int i;
        float t,sxoss,sx=0.0,sy=0.0,st2=0.0,ss,sigdat;

	*siga = 0.0;
	*sigb = 0.0;
	*a = 0.0;
        *b = 0.0;
	if (ndata <= 1) {
		return;
	}

	if (ndata == 2) {
		*b = (y[1]-y[0])/(x[1]-x[0]);
		*a = y[0] - (x[0] * (*b));
		return;
	}

	for (i=0;i<ndata;i++) {
		sx += x[i];
		sy += y[i];
	}
	ss=ndata;
        sxoss=sx/ss;
	for (i=0;i<ndata;i++) {
		t=x[i]-sxoss;
		st2 += t*t;
		*b += t*y[i];
	}
        *b /= st2;
        *a=(sy-sx*(*b))/ss;
        *siga=sqrt((1.0+sx*sx/(ss*st2))/ss);
        *sigb=sqrt(1.0/st2);
        *chi2=0.0;
	for (i=1;i<=ndata;i++) {
                *chi2 += SQR(y[i]-(*a)-(*b)*x[i]);
	}
        sigdat=sqrt((*chi2)/(ndata-2));
        *siga *= sigdat;
        *sigb *= sigdat;
}

/***************************************************************/
void four1(
	double data[],
	int nn,
	int isign
)
{
        unsigned long n,mmax,m,j,istep,i;
        double wtemp,wr,wpr,wpi,wi,theta;
        double tempr,tempi;

        n=nn << 1;
        j=1;
        for (i=1;i<n;i+=2) {
                if (j > i) {
                        SWAP(data[j],data[i]);
                        SWAP(data[j+1],data[i+1]);
                }
                m=n >> 1;
                while (m >= 2 && j > m) {
                        j -= m;
                        m >>= 1;
                }
                j += m;
        }
        mmax=2;
        while (n > mmax) {
                istep=mmax << 1;
                theta=isign*(6.28318530717959/mmax);
                wtemp=sin(0.5*theta);
                wpr = -2.0*wtemp*wtemp;
                wpi=sin(theta);
                wr=1.0;
                wi=0.0;
                for (m=1;m<mmax;m+=2) {
                        for (i=m;i<=n;i+=istep) {
                                j=i+mmax;
                                tempr=wr*data[j]-wi*data[j+1];
                                tempi=wr*data[j+1]+wi*data[j];
                                data[j]=data[i]-tempr;
                                data[j+1]=data[i+1]-tempi;
                                data[i] += tempr;
                                data[i+1] += tempi;
                        }
			wtemp = wr;
                        wr=wr*wpr-wi*wpi+wr;
                        wi=wi*wpr+wtemp*wpi+wi;
                }
                mmax=istep;
        }
}

/***************************************************************/
void gaussj(a,n,b,m)
double **a,**b;
int m,n;
{
        int *indxc,*indxr,*ipiv;
        int i,icol=0,irow=0,j,k,l,ll;
        double big,dum,pivinv;

	indxc = (int *) calloc (n, sizeof(int));
	indxr = (int *) calloc (n, sizeof(int));
	ipiv = (int *) calloc (n, sizeof(int));


	for (i=0; i<n; i++) {
		big = 0.0;
		for (j=0; j<n; j++) {
			if (ipiv[j] != 1) {
				for (k=0; k<n; k++) {
					if (ipiv[k] == 0) {
						if (fabs (a[j][k]) >= big) {
							big = fabs(a[j][k]);
							irow = j;
							icol = k;
						}
					} else if (ipiv[k] > 1) {
						nrerror ("gaussj: Singular Matrix-1");
					}
				}
			}
		}
		++(ipiv[icol]);
		if (irow != icol) {
			for (l=0; l<n; l++) SWAP(a[irow][l], a[icol][l])
			for (l=0; l<m; l++) SWAP(b[irow][l], b[icol][l])
		}
		indxr[i] = irow;
		indxc[i] = icol;
		if (a[icol][icol] == 0.0) {
			nrerror("gaussj: Singular Matrix-2");
		}

		pivinv = 1.0/a[icol][icol];
		a[icol][icol] = 1.0;
		for (l=0; l<n; l++) a[icol][l] *= pivinv;
		for (l=0; l<m; l++) b[icol][l] *= pivinv;
		for (ll=0; ll<n; ll++) {
			if (ll != icol) {
				dum = a[ll][icol];
				a[ll][icol] = 0.0;
				for (l=0; l<n; l++) a[ll][l] -= a[icol][l]*dum;
				for (l=0; l<m; l++) b[ll][l] -= b[icol][l]*dum;
			}
		}
	}

	for (l=n-1; l>=0; l--) {
		if (indxr[l] != indxc[l]) {
			for (k=0; k<n; k++) SWAP(a[k][indxr[l]],a[k][indxc[l]]);
		}
	}

	free (indxr);
	free (indxc);
	free (ipiv);
}





/****************************************************************/
/*
 * Calculate an interpolated value using results from a previous
 * call to 'spline'.
 * Checks if the requested time at which to calculate the 
 * interpolated value is out of range of the first data point.
 * If so, check if it is on the same day as the first data 
 * point, and use that value, else use 0.
 * This is needed for exporting values, since users often want
 * data starting at same day of first data point, but time is
 * before first data point time.
 */
/****************************************************************/
double interpolate (
	double x[], 
	double y[], 
	int n,
	double s2[], 
	double xp
)
{
	int yr, mon, day, hour, min, sec;
	int yr1, mon1, day1, hour1, min1, sec1;
	double s1, sd;

/*
 * If requested time is less than the first available data point,
 * check if it is on the same day.  If it is, return the y value,
 * else return 0.
 */
	if (xp < x[0]) {
		GetCalendarDate(xp, &yr, &mon, &day, &hour, &min, &sec);
		GetCalendarDate(x[0], &yr1, &mon1, &day1, &hour1, &min1, &sec1);
		if (yr == yr1 && mon == mon1 && day == day1)
			return (y[0]);
		else
			return (0);

	}

/*
 * If requested time is after last data point, return 0.
 */

	if (xp > x[n-1]) return (0);

	splint (x, y, s2, n, xp, &s1, &sd);
	return (s1);
}


/**********************************************************************/
/* General linear least squares fit to a function.
 *
 * Modified from the numerical recipes routine, to 
 * use double precision instead of float, and to 
 * explicitly use 0 offset arrays.  Also uses all 
 * parameters in the fit, rather than a selected number
 * like in the numerical recipes version.
 */
/**********************************************************************/
void lfit(
	double x[],		/* Input - the x axis values */
	double y[],		/* Input - the y axis values */
	double sig[],		/* Input - the uncertainty for each x,y pair */
	int ndat,		/* Input - the number of points in x,y,sig */
	float a[],		/* Output - the coefficients of the fit */
	double **covar,		/* Output - the covariances of the coefficients */
	int ma,			/* Input - Number of coefficients to fit */
	float *chisq,		/* Output - the chi square value of the fit */
	void (*funcs)()		/* Input - The function that returns the partial derivative
			           of the function at a given time x */
)

{
        int i,j,k,l,m;
        double ym,wt,sum, sig2i;
	double **beta, *afunc;

	beta = dmatrix (0,ma,0, 0);
	afunc = (double *) calloc (ma, sizeof(double));

/* Initialize arrays to 0 */


	for (j=0; j<ma; j++) {
		for (k=0; k<ma; k++) 
			covar[j][k] = 0.0;
		beta[j][0] = 0.0;
	}

	for (i=0; i<ndat; i++) {
		(*funcs)(x[i], afunc, ma);
		ym = y[i];
		sig2i = 1.0/(sig[i]*sig[i]);
		j=0;
		for (l=0; l<ma; l++) {
			wt=afunc[l]*sig2i;
			k=0;
			for (m=0; m<=l; m++) {
				covar[j][k] += wt*afunc[m];
				k++;
			}
			beta[j][0] += ym*wt;
			j++;
		}
	}

	for (j=1; j<ma; j++) {
		for (k=0; k<j; k++)
			covar[k][j] = covar[j][k];
	}

	gaussj (covar, ma, beta, 1);

	for (j=0; j<ma; j++) 
		a[j] = beta[j][0];

        *chisq=0.0;
        for (i=0; i<ndat; i++) {
                (*funcs)(x[i],afunc,ma);
		sum = 0.0;
                for (j=0; j<ma; j++) sum += a[j]*afunc[j];
                *chisq += SQR((y[i]-sum)/sig[i]);
        }
        covsrt(covar,ma);

	free_dmatrix (beta, 0, ma, 0, 0);
	free (afunc);
}

/*****************************************************************/
static void covsrt(covar,ma)
double **covar;
int ma;
{
        int i,j,k;
        double swap;

        k=ma-1;
        for (j=ma-1;j>=0;j--) {
		for (i=0; i<ma; i++) SWAP(covar[i][k],covar[i][j])
		for (i=0; i<ma; i++) SWAP(covar[k][i],covar[j][i])
		k--;
        }
}

/***************************************************************/
void realft(
	double data[],
	int n,
	int isign
)
{
        void four1();
        unsigned long i,i1,i2,i3,i4,np3;
        double c1=0.5,c2,h1r,h1i,h2r,h2i;
        double wr,wi,wpr,wpi,wtemp,theta;

        theta=3.141592653589793/(double) (n>>1);
        if (isign == 1) {
                c2 = -0.5;
                four1(data,n>>1,1);
        } else {
                c2=0.5;
                theta = -theta;
        }
        wtemp=sin(0.5*theta);
        wpr = -2.0*wtemp*wtemp;
        wpi=sin(theta);
        wr=1.0+wpr;
        wi=wpi;
        np3=n+3;
        for (i=2;i<=(n>>2);i++) {
                i4=1+(i3=np3-(i2=1+(i1=i+i-1)));
                h1r=c1*(data[i1]+data[i3]);
                h1i=c1*(data[i2]-data[i4]);
                h2r = -c2*(data[i2]+data[i4]);
                h2i=c2*(data[i1]-data[i3]);
                data[i1]=h1r+wr*h2r-wi*h2i;
                data[i2]=h1i+wr*h2i+wi*h2r;
                data[i3]=h1r-wr*h2r+wi*h2i;
                data[i4] = -h1i+wr*h2i+wi*h2r;
                wr=(wtemp=wr)*wpr-wi*wpi+wr;
                wi=wi*wpr+wtemp*wpi+wi;
        }
        if (isign == 1) {
                data[1] = (h1r=data[1])+data[2];
                data[2] = h1r-data[2];
        } else {
                data[1]=c1*((h1r=data[1])+data[2]);
                data[2]=c1*(h1r-data[2]);
                four1(data,n>>1,-1);
		for (i=1; i<=n; i++) data[i] = data[i]*2.0/(float) n;
        }
}


/*************************************************************************/
/* Given arrays x[0..n-1] and y[0..n-1] containing a function, 
 * return the array y2[0..n-1] that contains the second derivatives of 
 * the interpolating function at the tabulated points x. 
 * This routine sets the boundary condition for a natural spline
 * with 0 second derivative on the boundaries.
 */
void spline(
	double x[],
	double y[],
	int n,
	double y2[]
)
{
	int i,k;
	double p,qn,sig,un,*u;

	u=(double *) calloc (n, sizeof(double));
	y2[0]=u[0]=0.0;
	for (i=1;i<=n-2;i++) {
		sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
		p=sig*y2[i-1]+2.0;
		y2[i]=(sig-1.0)/p;
		u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		u[i]=(6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
	}
	qn=un=0.0;
	y2[n-1]=(un-qn*u[n-2])/(qn*y2[n-2]+1.0);
	for (k=n-2;k>=0;k--)
		y2[k]=y2[k]*y2[k+1]+u[k];
	free (u);
}

/*******************************************************/
void splint (
	double xa[],
	double ya[],
	double y2a[],
	int n,
	double x,
	double *y,
	double *yd
)
{
	int klo, khi, k;
	double h, b, a, g;

/* 
 * Find correct position by bisection
 * khi and klo should bracket the value of x
 */

	klo = 0;
	khi = n-1;
	while (khi - klo > 1) {
		k = (khi+klo) >> 1;
		if (xa[k] > x) 
			khi = k;
		else
			klo=k;
	}

	h = xa[khi] - xa[klo];
	g = ya[khi] - ya[klo];
	if (h == 0.0) {
		fprintf (stderr, "Bad xa input to routine splint\n");
		exit(1);
	}

	a = (xa[khi] - x) / h;
	b = (x - xa[klo]) / h;
	
/* calculate the function */

        *y = a*ya[klo] + b*ya[khi] + ((a*a*a-a)*y2a[klo] + (b*b*b-b)*y2a[khi]) * (h*h) / 6.0;

/* calculate the derivative */
	
	*yd = g/h - (((3.0*a*a - 1.0) * y2a[klo] + (3*b*b - 1) * y2a[khi]) * h) / 6.0;

	return;
}


#define NR_END 1
#define FREE_ARG char*

/***********************************************************************/
/* Numerical Recipes standard error handler */
/***********************************************************************/
void nrerror(error_text)
char error_text[];
{
        void exit();

        fprintf(stderr,"Numerical Recipes run-time error...\n");
        fprintf(stderr,"%s\n",error_text);
        fprintf(stderr,"...now exiting to system...\n");
        exit(1);
}

/***********************************************************************/
/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
/***********************************************************************/
double **dmatrix(nrl,nrh,ncl,nch)
long nch,ncl,nrh,nrl;
{
        long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
        double **m;

        /* allocate pointers to rows */
        m=(double **) malloc((unsigned int)((nrow+NR_END)*sizeof(double*)));
        if (!m) nrerror("allocation failure 1 in matrix()");
        m += NR_END;
        m -= nrl;

        /* allocate rows and set pointers to them */
        m[nrl]=(double *) malloc((unsigned int)((nrow*ncol+NR_END)*sizeof(double)));
        if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
        m[nrl] += NR_END;
        m[nrl] -= ncl;

        for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;

        /* return pointer to array of pointers to rows */
        return m;
}

/***********************************************************************/
/* free a double matrix allocated by dmatrix() */
/***********************************************************************/
void free_dmatrix(m,nrl,nrh,ncl,nch)
double **m;
long nch,ncl,nrh,nrl;
{
        free((FREE_ARG) (m[nrl]+ncl-NR_END));
        free((FREE_ARG) (m+nrl-NR_END));
}