/*************************************************************************
* (last 4/9/91) (old date: 1/30/91)                                      *
*                                                                        *
* Numerical solution routines for the Bailey-Makeham model               *
*   Commissioned by: R. Clifton Bailey, HCFA                             *
*                    Henry Krakauer, HCFA HSQB                           *
*   Based on code written by Jim Summe, NIH                              *
*   Author, Bill Rogers: Rand, Computing Resource Center                 *
*************************************************************************/

#include <math.h>
#include "bailey.h"

/*************************************************************************
*   The procedure "Solve" solves the system of equations TT'X=B where T  *
*   is a lower-triangular non-singular matrix, X is a vector of unknowns,*
*   and B is a vector of constants.  N is the number of elements in X.   *
*                                                                        *
*   L is in standard lower-triangular form (adjoined rows of T).         *
*                                                                        *
*   The algorithm used is based on Algorithms 1.3 and 4.5 in Chapter 3   *
*   of GW Stewart's Introduction to Matrix Computations published by     *
*   Academic Press in 1973.                                              *
*************************************************************************/

void solve(l,x,b,n)
DOUBLE *l, *x, *b;
int n;
{
	int index, i, j, high;
	/* Let z = t'x; Solve tz=b and store z in x. */

	for (index=i=0; i<n; i++) {
		x[i] = b[i];
		/* Subtract the cross products between the off-diagonal
		   elements in the i-th row of t and the known elements of z
		*/
		for (j=0; j<i; j++,index++) x[i] -= x[j]*l[index];
		/* Divide by the diagonal element in the i-th row of t */
		x[i] /= l[index++];
	}
	/* Solve t'x=z in reverse order */
	--index;
	for (high=index,i=n-1; i>=0; i--,index=high) {
		/* Subtract the cross-products between the off-diagonal
		   elements in the i-th row of t' and the known elements of X.
		*/
		for (j=n-1; j>i; j--) {
			x[i] -= x[j]*l[index];
			index -= j;
		}
		x[i] /= l[index];
		--high;
	}
}

/*************************************************************************
*  This procedure performs a Cholesky factorization of "a".  The         *
*  algorithm used is a slight modification of Algorithm 3.9 in Chapter 3 *
*  of GW Stewart's "Introduction to Matrix Computation", Academic Press, *
*  1973.                                                                 *
*                                                                        *
*                                                                        *
*  "a" is the vector containing the adjoined rows of the lower           *
*  triangular half of the symmetric positive definite matrix to          *
*  be factored.                                                          *
*                                                                        *
*  "n" is the number of rows/columns in the matrix to be factored.       *
*                                                                        *
*  "l" is the vector which returns the adjoined rows of the lower        *
*  triangular matrix generated by the factorization.                     *
*                                                                        *
*  The return value is the row number in which non-positive              *
*  definiteness was first indicated.  If the matrix was successfully     *
*  factored, the return is -1, otherwise row number.                     *
*                                                                        *
*************************************************************************/

int cholsky(a, n, l)
DOUBLE *a;
int n;
DOUBLE *l;
{
	int cei, cpi, i, j, msi;

	for (i=cei=0; i<n; ++i) {
		for (msi=j=0; j<=i; ++j, ++cei) {
			l[cei] = 0.0;
			/* Accumulate the Cross-Product corrections */
			for (cpi=cei-j; cpi<cei; ++cpi) l[cei] += l[cpi]*l[msi++];
			l[cei] = a[cei]-l[cei];

			if (i==j) {

				/* Check for positive definiteness

				The following test insures against a
				non-positive definite matrix appearing
				numerically as a positive definite matrix.

				The bound used is based in part on an upper	
				bound for the inner product found in Stewart
				Appendix 3. */
				
				if (l[cei]>a[cei]*FUZZ*(i+1)) l[cei]=sqrt(l[cei]);
				else return i;
			} else l[cei] /= l[msi++];
		}
	}
	return -1;
}

/************************************************************************
* Standard matrix inverter, using Cholesky decomposition                *
************************************************************************/

int matinvert(a,fa,work,n) DOUBLE *a, *fa, *work; int n; {
	int j, k, i;
	j = cholsky (a, n, fa);
	if (j>=0) {
		return j;
	}
	for (j=k=0; k<n; ++k) {
		for (i=0; i<n; ++i) work[i]=0.0;
		work[k] = 1.0;
		solve(fa,work,work,n);
		for (i=0; i<=k; ++i) a[j++] = work[i];
	}
	return (-1);
}



/************************************************************************
* fps (x, f1, f2)                                                       *
*                                                                       *
*         Calculates: f1 = exp(-x)                                      *
*                     f2 = 1 - exp(-x)                                  *
*             assumes that x >= 0.                                      *
*                                                                       *
************************************************************************/

void fps(x, f1, f2)
LDOUBL x, *f1, *f2;
{
	LDOUBL term[9];
	if (x >= 300.0) {
		*f1 = 0.;
		*f2 = 1.;
		return;
	}
	if (x >= 36.74) {
		*f1 = exp(-x);
		*f2 = 1.;
		return;
	} 
	if (x >= .06454) {
		*f1 = exp(-x);
		*f2 = 1.- *f1;
		return;
	} else {
		/* *** Use a Taylor Series expansion *** */
		term[1] = x*x/2.;
		term[2] = term[1]*x/3.;
		term[3] = term[2]*x/4.;
		term[4] = term[3]*x/5.;
		term[5] = term[4]*x/6.;
		term[6] = term[5]*x/7.;
		term[7] = term[6]*x/8.;
		term[8] = term[7]*x/9.;
		*f2 = term[8]*x/10.;
		*f2 = *f2 + term[8];
		*f2 = *f2 + term[7];
		*f2 = *f2 + term[6];
		*f2 = *f2 + term[5];
		*f2 = *f2 + term[4];
		*f2 = *f2 + term[3];
		*f2 = *f2 + term[2];
		*f2 = *f2 + term[1];
		*f2 = *f2 + x;
		*f1 = *f2 + 1.;
		if (*f2 >= FUZZ) {
			*f1 = 1./ (*f1);
			*f2 *= (*f1);
	    }
	}
}

/************************************************************************
* sps (x, f1, f2)                                                       *
*                                                                       *
*         Calculates: f1 = exp(-x)                                      *
*                     f2 = 1 - exp(-x)                                  *
*                     f3 = 1 - (1+x)*exp(-x)                            *
*         assumes that x >= 0.                                          *
************************************************************************/

void sps(x, f1, f2, f3)
LDOUBL x, *f1, *f2, *f3;
{
	LDOUBL term[14];
	if (x >= 300.0) {
		*f1 = 0.;
		*f2 = 1.;
		*f3 = 1.;
		return;
	}
	if (x >= 40.462) {
		*f1 = exp(-x);
		*f2 = 1.;
		*f3 = 1.;
		return;
	} else {
		if (x >= .40353) {
			*f1 = exp(-x);
			*f2 = 1. - *f1;
			*f3 = *f2 - x*(*f1);
		} else {
			/* *** Use a Taylor's series expansion *** */
			term[1] = x*x/2.;
			term[2] = term[1]*x/3.;
			term[3] = term[2]*x/4.;
			term[4] = term[3]*x/5.;
			term[5] = term[4]*x/6.;
			term[6] = term[5]*x/7.;
			term[7] = term[6]*x/8.;
			term[8] = term[7]*x/9.;
			term[9] = term[8]*x/10.;
			term[10] = term[9]*x/11.;
			term[11] = term[10]*x/12.;
			term[12] = term[11]*x/13.;
			term[13] = term[12]*x/14.;
			*f3 = term[13]*x/15.;
			*f3 += term[13];
			*f3 += term[12];
			*f3 += term[11];
			*f3 += term[10];
			*f3 += term[9];
			*f3 += term[8];
			*f3 += term[7];
			*f3 += term[6];
			*f3 += term[5];
			*f3 += term[4];
			*f3 += term[3];
			*f3 += term[2];
			*f3 += term[1];
			*f2 = *f3 + x;
			*f1 = *f2 + 1.;
			if (*f2 >= FUZZ) {
				*f1 = 1./ *f1;
				*f2 = *f2 * *f1;
				*f3 = *f3 * *f1;
			}
		}
	}
}

/***********************************************************************
* The following routines compute useful mathematical functions         *
***********************************************************************/

#define TWODPI  0.6366197723675813430755351
#define SQR2PI  2.506628274631000502415765

double  lngamma(x)
double  x;
{
	double xx, tmp, ser;
	static double cof[6] = {76.18009173, -86.50532033, 24.01409822, -1.231739516, 
		0.120858003e-2, -0.536382e-5};
	int j;
	xx = x - 1.0;
	tmp = xx + 5.5;
	tmp -= (xx+0.5)*log(tmp);
	ser = 1.0;
	for (j=0; j<6; ++j) {
		xx += 1.0;
		ser += cof[j]/xx;
	}
	return -tmp + log(SQR2PI*ser);
}

#define ITMAX 100
#define EPS 1e-6

double  cfbeta(a,b,x)
double  a, b, x;
{
	double qap, qam, qab, em, tem, d;
	double bz, bm=1.0, bp, bpp;
	double az=1.0, am=1.0, ap, app, aold;
	int m;
	
	qab = a+b;
	qap = a+1.0;
	qam = a-1.0;
	bz = 1.0 - qab*x/qap;
	for (m=1; m<=ITMAX; m++) {
		em = m;
		tem = em+em;
		d = em*(b-em)*x/((qam+tem)*(a+tem));
		ap = az + d*am; bp = bz + d*bm;
		d = -(a+em)*(qab+em)*x/((qap+tem)*(a+tem));
		app = ap + d*az;  bpp = bp + d*bz;
		aold = az;
		am = ap/bpp;
		bm = bp/bpp;
		az = app/bpp;
		bz = 1.0;
		if (fabs(az-aold) < (EPS*fabs(az))) return az;
	}
	return az;
}


double  ibeta(a,b,x)
double  a, b, x;
{
	double cf;
	if (x<=0.0 || x>=1.0) cf = 0.0;
	else cf = exp(lngamma(a+b)-lngamma(a)-lngamma(b)+a*log(x)+b*log(1.0-x));
	if (x<(a+1.0)/(a+b+2.0)) return cf*cfbeta(a,b,x)/a;
	else return 1.0-cf*cfbeta(b,a,1.0-x)/b;
}

double tprob(x, df) double x, df; {
        return  ibeta(df/2.0,0.5,df/(df+x*x));
}

double chiprob(x, df) double x, df; {
		if (x<=0.0) return 1.0;
		return ibeta(250.0, df/2.0, 250.0/(250 + x));
}

double fprob(f, df1, df2) double f, df1, df2; {
		return ibeta(df2/2.0, df1/2.0, df2/(df2+df1*f));
}