/* SIMPDEV.C */
/*-
quadratic fit for standard deviations	= simpdev()
  
J.A. Rupley, Tucson, Arizona
rupley!local@megaron.arizona.edu
*/

#define SIMPDEV

#include "simpdefs.h"

/*-
SIMPDEV
	QUADRATIC FIT TO FUNCTION SURFACE FOR ESTIMATION OF:
		VARIANCE-COVARIANCE MATRIX 
		STANDARD DEVIATIONS OF PARAMETERS
*/

int
simpdev(fptr)
	FILE           *fptr;
{
	register int    i, j, k, l;
	int             xfree, free_cnt;
	int             err_count;

	double          dtemp;
	double          yminusij, yplusij;

	double          tmparray[NPARM];

	struct pstruct  ptemp;

	extern double   sqrt();

	extern          func();
	extern void     fpprint();
	extern void     pcopy();
	extern void     fqprint();
	extern void     fmatprint();

	extern void     qsort();
	extern          pvalcomp();

	/* set pcent.val */
	if (func(&pcent) == ERROR)
		return (ERROR);

	/*
	 * ascending sort of pointers p_p to struct array p 
	 */
	qsort((char *) p_p, (unsigned) nvert, sizeof(*p_p), pvalcomp);

	/*
	 * if lowest vertex < centroid, then replace centroid by lowest
	 * vertex 
	 */
	if (p_p[0]->val < pcent.val) {
		pcopy(&pcent, p_p[0]);
		fprintf(fptr, "\n\nlowest vertex replaces centroid");
	}
	yzero = pcent.val;
	fprintf(fptr,
		"\n\nlowest function value = yzero = %20.14e\n\n", yzero);
	fpprint(fptr, &pcent);
	fprintf(fptr, "\n");

	/*
	 * calculate q value = avg devn of a free parameter from the centroid
	 * value; 
	 *
	 * store the q value in structure q : q.q[free_cnt] = q value; 
	 *
	 * also store map of free parm to parm array in q.parmndx[free_cnt] =
	 * index of parm in p[nvert].parm[nparm]; 
	 *
	 * set nfree and nvert 
	 */
	free_cnt = 0;
	for (i = 0; i < nparm; i++) {
		dtemp = 0;
		for (j = 0; j < nvert; j++)
			dtemp = dtemp + ABS(p[j].parm[i] - pcent.parm[i]);
		dtemp = dtemp / nvert;
/* 		if (ABS(dtemp / pcent.parm[i]) < 1.0e-16) { */
		if (dtemp == 0) {
			fprintf(fptr, "parameter %d is fixed\n", i);
			continue;
		} else {
			q.q[free_cnt] = dtemp;
			q.parmndx[free_cnt] = i;
			free_cnt++;
		}
	}
	nfree = free_cnt;
	if (nvert != (nfree + 1)) {
		fprintf(fptr, "\nerror in count of free parameters\n");
		return (ERROR);
	}
	fprintf(fptr,
		"\nq values for the %d free parameters out of %d total:\n",
		nfree, nparm);
	fqprint(fptr, q.q);

	/*
	 * check and if necessary adjust q values for each free parameter, to
	 * be sure that the function values are OK for the centroid parameter
	 * values  (+  &  -)  q; 
	 *
	 * store function value for parameter value (+  &  -)  q   in  q.yplus 
	 * and  q.yminus 
	 */
	for (i = 0; i < nfree; i++) {
		k = q.parmndx[i];
		pcopy(&ptemp, &pcent);
		fprintf(fptr,
			"\nchecking q value %d, for parameter %d: ", i, k);
		while (TRUE) {
			fprintf(fptr, "*");

			while (TRUE) {
				ptemp.parm[k] = pcent.parm[k] - q.q[i];
				if (func(&ptemp) == ERROR)
					q.q[i] = q.q[i] / 2;
				else
					break;
			}
			q.yminus[i] = ptemp.val;

			dtemp = q.q[i];
			while (TRUE) {
				ptemp.parm[k] = pcent.parm[k] + dtemp;
				if (func(&ptemp) == ERROR)
					dtemp = dtemp / 2;
				else
					break;
			}
			q.yplus[i] = ptemp.val;

			if (dtemp == q.q[i])
				break;
			else
				q.q[i] = dtemp;
		}
	}
	fprintf(fptr, "\n\nadjusted q values:\n");
	fqprint(fptr, q.q);
	fprintf(fptr, "\nyplus values:\n");
	fqprint(fptr, q.yplus);
	fprintf(fptr, "\nyminus values:\n");
	fqprint(fptr, q.yminus);

	/*
	 * calculate  <a>  vector; store in q.a; 
	 *
	 * calculate  <B>  matrix diagonal elements; 
	 *
	 * store in  qmat  and  q.bdiag  
	 */
	for (i = 0; i < nfree; i++) {
		q.a[i] = 0.25 * (q.yplus[i] - q.yminus[i]);
		qmat[i][i] = q.bdiag[i] = 0.5 * (q.yplus[i] + q.yminus[i]
						 - 2 * yzero);
	}
	fprintf(fptr, "\n<a> vector:\n");
	fqprint(fptr, q.a);
	fprintf(fptr,
		"\n<B> matrix diagonal:\n");
	fqprint(fptr, q.bdiag);

	/*
	 * calculate <B>  matrix off-diagonal elements; 
	 *
	 * keep track of any function errors; 
	 *
	 * store results in  qmat  
	 */
	err_count = 0;
	for (i = 1; i < nfree; i++) {
		k = q.parmndx[i];
		fprintf(fptr,
			"\ncalculating off-diag Bij for row %d: ", i);
		for (j = 0; j < i; j++) {
			fprintf(fptr, "*");
			l = q.parmndx[j];
			pcopy(&ptemp, &pcent);

			ptemp.parm[k] = pcent.parm[k] + q.q[i];
			ptemp.parm[l] = pcent.parm[l] + q.q[j];
			if (func(&ptemp) == ERROR)
				++err_count;
			yplusij = ptemp.val;

			ptemp.parm[k] = pcent.parm[k] - q.q[i];
			ptemp.parm[l] = pcent.parm[l] - q.q[j];
			if (func(&ptemp) == ERROR)
				++err_count;
			yminusij = ptemp.val;

			qmat[j][i] = qmat[i][j] = 0.25 * (yplusij + yminusij
				      + 2 * yzero - q.yplus[i] - q.yminus[i]
						- q.yplus[j] - q.yminus[j]);
		}
	}
	if (err_count > 0) {
		fprintf(fptr, "\n\n%d function errors in <B> matrix calculation\n",
			err_count);
		return (ERROR);
	}
	fprintf(fptr, "\n\n<B> matrix:\n");
	fmatprint(fptr, qmat);

	/*
	 * invert <B>  matrix  
	 */
	xfree = nfree - 1;
	for (l = 0; l < nfree; l++) {
		tmparray[xfree] = 1 / qmat[0][0];

		for (i = 0; i < xfree; i++)
			tmparray[i] = tmparray[xfree] * qmat[0][i + 1];

		for (i = 0; i < xfree; i++) {
			qmat[i][xfree] = -tmparray[xfree] * qmat[i + 1][0];
			for (j = 0; j < xfree; j++)
				qmat[i][j] = qmat[i + 1][j + 1]
					- tmparray[j] * qmat[i + 1][0];
		}

		for (i = 0; i < nfree; i++)
			qmat[xfree][i] = tmparray[i];
	}
	fprintf(fptr, "\n<B> matrix inverse:\n");
	fmatprint(fptr, qmat);


	/* store inv B matrix diagonal in q.inv_bdiag  */
	/* calculate: ymin = yzero - sumij(ai * invBij * aj)  */
	/* calculate: pmin(k) = centroid(k) - sumj(qi * invBij * aj)  */
	/* where k = parmndx[i] if parm[k] free and  */
	/* pmin = centroid if parm[k] fixed  */
	ymin = yzero;
	pcopy(&pmin, &pcent);
	for (i = 0; i < nfree; i++) {
		q.inv_bdiag[i] = qmat[i][i];
		k = q.parmndx[i];
		for (j = 0; j < nfree; j++) {
			ymin = ymin - q.a[i] * qmat[i][j] * q.a[j];
			pmin.parm[k] = pmin.parm[k]
				- q.q[i] * qmat[i][j] * q.a[j];
		}
	}
	if (func(&pmin) == ERROR)
		fprintf(fptr, "\nerror in y-pmin\n");
	ypmin = pmin.val;

	/* calculate  mse = (func val)/degrees freedom & rms error */
	/* calculate  variance-covariance matrix */
	/* vcij = qi * invBij * qj * mse */
	/* calculate  standard deviations */
	mse = pcent.val / (ndata - nfree);
	rms_data = sqrt(mse);
	for (i = 0; i < nfree; i++) {
		for (j = i; j < nfree; j++) {
			qmat[i][j] = qmat[j][i]
				= q.q[i] * qmat[i][j] * q.q[j] * mse;
		}
		q.std_dev[i] = qmat[i][i] < 0 ? -1 : sqrt(qmat[i][i]);
	}
	fprintf(fptr,
		"\nvariance-covariance matrix:\n");
	fmatprint(fptr, qmat);

	/*
	 * replace centroid with pmin, if y-pmin  <  y-centroid; 
	 *
	 * reconstruct simplex, based on q values and centroid, in preparation
	 * for more fitting; 
	 *
	 * nfree vertices are based on nfree q values; 
	 *
	 * the remaining vertex is set at the centroid (or pmin if it is lower) 
	 */
	fprintf(fptr, "\n\nCalculating for reconstructed simplex...\n\n");
	if (pmin.val < pcent.val)
		pcopy(&p[nfree], &pmin);
	else
		pcopy(&p[nfree], &pcent);
	for (i = 0; i < nfree; i++) {
		pcopy(&p[i], &pcent);
		k = q.parmndx[i];
		p[i].parm[k] = p[i].parm[k] + q.q[i];
		func(&p[i]);
	}

	return (OK);
}				/* END OF SIMPDEV      			 */