/* SIMPDEV.C */
/*-
quadratic fit to function surface for estimation of:
	variance-covariance matrix
	standard deviation of parameters

J.A. Rupley, Tucson, Arizona
rupley!local@cs.arizona.edu
*/
/*-
QUADRATIC FIT TO THE LEAST SQUARES FUNCTION SURFACE

Standard deviations of the parameters are calculated by fitting a
quadratic function to the surface about the minimum and then using the
properties of this function to calculate the variance-covariance
matrix.  The strategy is that of Nelder and Mead (1965), with a
modification of the method of estimating the derivatives.

The contracted simplex obtained through the minimization process is
used to define a new coordinate system of M axes in parameter
space.  The origin is at the centroid of the simplex.  Each axis
corresponds to a free parameter.  The unit value (the scale) for each
axis is set in the new system at the average of the absolute values of
the deviations of the vertices of the simplex from the centroid value.

The diagonal matrix Q transforms the new coordinate system back to the
original system.  

The function value, y, in the region near the function minimum can be
approximated in the new coordinate system by the quadratic vector
function:

      y  =  a(0)   +   2 a'.x   +   x'.B.x                   (1)

The vector x defines a point in parameter space.  The scalar a(0), the
vector a and the matrix B are determined by the shape of the surface y
near the minimum and can be calculated with the use of simple numerical
approximations, evaluated at x = 0 (the position of the centroid of the
original simplex):

        a(0)   =   y(0)

        ai     =   0.5 * (dy/dxi)
               =   0.25 * (yi - y-i)

        bij    =   0.5 * (d2y/dxi.dxj)
               =   0.25 * (yij + y-i-j + 2 * y(0)
                                   - y-i - y-j - yi - yj)

        bii    =   0.5 * (d2y/dxi2)
               =   0.5 * (yi + y-i - 2 * y(0))

A refined estimate of the minimum position, based on the quadratic
approximation of equation (1) above, is:

     xmin = - B-1.a

The function minimum at xmin is:

     ymin = a(0) - a'.B-1.a

The minimum in the original coordinate system is
obtained by back transformation of xmin with Q:

     pmin = p(0) - Q'.B-1.a

where p(0) is the centroid.

At convergence there should be close agreement between the function
values y(xmin), y(pmin), and y(centroid), and between the parameter
vectors pmin and the centroid.

The hessian matrix in the original coordinate system is

     Q-1'.(2 * B).Q-1

The unscaled variance-covariance matrix C is given by

     C = Q.B-1.Q'

A diagonal element of C, multiplied by the mean square error (MSE),
gives the variance of the corresponding parameter:

     var(i) = cii * MSE
     MSE = ymin/(n - m)
     std-dev(i) = sqrt(var(i))

where (n - m) = # degrees freedom; for a least squares fit
to a set of data, n = # of data points, m = # of free parameters, and
ymin = the weighted sum of residuals squared.

Similarly,

     cov(ij) = cij * MSE

*/

#include "simpdefs.h"

int
simpdev(fp_out)
	FILE           *fp_out;
{
	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     pcopy();

	extern void     qsort();
	extern          pvalcomp();

	/* set pcent.val */
	if (func(&pcent) == ERROR) {
		if (verbose >= SLIGHTLY)
			fprintf(fp_out, "simpdev: error in function evaluation at centroid\n");
		return (ERROR);
	}

	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]);
	}
	yzero = pcent.val;

	/*
	 * reset and check nfree
	 * 
	 * calculate q = avg devn of a free parameter from the centroid;
	 * 
	 * store map of free parm to parm array in q.parmndx[free_cnt] = index
	 * of parm in p[nvert].parm[nparm];
	 */
	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) {
			continue;
		} else {
			q.q[free_cnt] = dtemp;
			q.parmndx[free_cnt] = i;
			free_cnt++;
		}
	}
	nfree = free_cnt;
	if (nvert != (nfree + 1)) {
		if (verbose >= SLIGHTLY)
			fprintf(fp_out, "simpdev: bad simplex, nvert != nfree + 1 \n");
		return (ERROR);
	}
	/*
	 * adjust q.q values so that function values are not ERROR at parameter
	 * values (centroid + q) and (centroid - q)
	 * 
	 * the q.q[i] are the diagonal and only nonzero elements of the matrix
	 * Q, which transforms from the new back to the original coordinate
	 * system;
	 * 
	 * store function value for parameter value + q  and  - q   in  q.yplus
	 * and  q.yminus
	 */
	for (i = 0; i < nfree; i++) {
		k = q.parmndx[i];
		pcopy(&ptemp, &pcent);
		while (TRUE) {
			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;
		}
	}

	/*
	 * 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);
	}

	/*
	 * 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];
		for (j = 0; j < i; j++) {
			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) {
		if (verbose >= SLIGHTLY)
			fprintf(fp_out, "simpdev: %d error(s) in calculation of <B> matrix elements\n", err_count);
		return (ERROR);
	}
	/*
	 * calculate and store hessian = matrix of 2nd derivatives;
	 * 
	 * hessian = Qinv' * (B' + B) * Qinv in original coordinate system
	 */
	for (i = 0; i < nfree; i++)
		for (j = i; j < nfree; j++)
			hessian[i][j] = hessian[j][i] =
				1 / q.q[i] * 2 * qmat[i][j] * 1 / q.q[j];
	/*
	 * 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];
	}


	/* store inv B matrix diagonal in q.inv_bdiag  */
	/* calculate: ymin */
	/* calculate: pmin */
	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)
		/* error ok, even if not wished for */
		if (verbose >= SLIGHTLY)
			fprintf(fp_out, "simpdev: cannot evaluate function at pmin\n");
	ypmin = pmin.val;

	/* calculate MSE = (func val)/degrees freedom */
	/* calculate rms error */
	/* calculate variance-covariance matrix */
	/* scaled by the MSE and in the original coordinate system */
	/* vcij = qi * invBij * qj * MSE */
	/* return vc matrix in qmat */
	/* calculate standard deviations of the parameters */
	/* store in q.std_dev */
	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]);
	}

	if (prog_done)
		return (OK);

	/*
	 * replace centroid with pmin, if y-pmin  <  y-centroid;
	 * 
	 * reconstruct simplex, based on Q matrix and centroid, in preparation
	 * for more fitting;
	 * 
	 * nfree vertices are based on nfree q values, calculated as centroid
	 * (or pmin) + appropriate Q matrix element;
	 * 
	 * the remaining vertex is set at the centroid (or pmin if it is lower)
	 */
	if (pmin.val < pcent.val) {
		if (verbose >= SLIGHTLY)
			fprintf(fp_out, "simpdev: reconstruct simplex from pmin\n");
		pcopy(&p[nfree], &pmin);
	} else {
		if (verbose >= SLIGHTLY)
			fprintf(fp_out, "simpdev: reconstruct simplex from old centroid\n");
		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      			 */