/*  minfit.c -- Minimum fit. Translated to 'C' from the
 *           original Algol code in "Handbook for Automatic Computation,
 *           vol. II, Linear Algebra", Springer-Verlag.
 *
 *  (C) 2000, C. Bond. All rights reserved.
 *
 */
#include <malloc.h>  /* for allocation of vector 'e' */
#include <math.h>    /* for 'fabs'            */

void minfit(int m,int n,int p,double eps,double tol,
	double **ab,double *q)
{
	int i,j,k,l,l1,n1,np;
	double c,f,g,h,s,x,y,z;
	double *e;

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

/* Householder's reduction to bidiagonal form. */
	g = x = 0.0;    
	np = n + p;
	for (i=0;i<n;i++) {
		e[i] = g;
		s = 0.0;
		l = i + 1;
		for (j=i;j<m;j++)
			s += (ab[j][i]*ab[j][i]);
		if (s < tol)
			g = 0.0;
		else {
			f = ab[i][i];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			ab[i][i] = f - g;
			for (j=l;j<n;j++) {
				s = 0.0;
				for (k=i;k<m;k++)
					s += (ab[k][i] * ab[k][j]);
				f = s / h;
				for (k=i;k<m;k++)
					ab[k][j] += (f * ab[k][i]);
			} /* end j */
		} /* end s */
		q[i] = g;
		s = 0.0;
		if (i < m) {
			for (j=l;j<n;j++)
				s += (ab[i][j] * ab[i][j]);
		}
		if (s < tol)
			g = 0.0;
		else {
			f = ab[i][i+1];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			ab[i][i+1] = f - g;
			for (j=l;j<n;j++) 
				e[j] = ab[i][j]/h;
			for (j=l;j<m;j++) {
				s = 0.0;
				for (k=l;k<n;k++) 
					s += (ab[j][k] * ab[i][k]);
				for (k=l;k<n;k++)
					ab[j][k] += (s * e[k]);
			} /* end j */
		} /* end s */
		y = fabs(q[i]) + fabs(e[i]);                         
		if (y > x)
			x = y;
	} /* end i */

/* accumulation of right-hand transformations */
	for (i=n-1;i>=0;i--) {
		if (g != 0.0) {
			h = ab[i][i+1] * g;
			for (j=l;j<n;j++)
				ab[j][i] = ab[i][j]/h;
			for (j=l;j<n;j++) {
				s = 0.0;
				for (k=l;k<n;k++) 
					s += (ab[i][k] * ab[k][j]);
				for (k=l;k<n;k++)
					ab[k][j] += (s * ab[k][i]);
			} /* end j */
		} /* end g */
		for (j=l;j<n;j++)
			ab[i][j] = ab[j][i] = 0.0;
		ab[i][i] = 1.0;
		g = e[i];
		l = i;
	} /* end i */

	eps *= x;
	n1 = n + 1;
	for (i=m;i<n;i++) {
		for (j=n;j<np;j++) 
			ab[i][j] = 0.0;
	}
/* diagonalization of the bidiagonal form */
	for (k=n-1;k>=0;k--) {
test_f_splitting:
		for (l=k;l>=0;l--) {
			if (fabs(e[l]) <= eps) goto test_f_convergence;
			if (fabs(q[l-1]) <= eps) goto cancellation;
		} /* end l */

/* cancellation of e[l] if l > 0 */
cancellation:
		c = 0.0;
		s = 1.0;
		l1 = l - 1;
		for (i=l;i<=k;i++) {
			f = s * e[i];
			e[i] *= c;
			if (fabs(f) <= eps) goto test_f_convergence;
			g = q[i];
			h = q[i] = sqrt(f*f + g*g);
			c = g / h;
			s = -f / h;
			for (j=n;j<np;j++) {
				y = ab[l1][j];
				z = ab[i][j];
				ab[l1][j] = y * c + z * s;
				ab[i][j] = -y * s + z * c;
			} /* end j */
		} /* end i */
test_f_convergence:
		z = q[k];
		if (l == k) goto convergence;

/* shift from bottom 2x2 minor */
		x = q[l];
		y = q[k-1];
		g = e[k-1];
		h = e[k];
		f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
		g = sqrt(f*f + 1.0);
		f = ((x-z)*(x+z) + h*(y/((f<0)?(f-g):(f+g))-h))/x;
/* next QR transformation */
		c = s = 1.0;
		for (i=l+1;i<=k;i++) {
			g = e[i];
			y = q[i];
			h = s * g;
			g *= c;
			e[i-1] = z = sqrt(f*f+h*h);
			c = f / z;
			s = h / z;
			f = x * c + g * s;
			g = -x * s + g * c;
			h = y * s;
			y *= c;
			for (j=0;j<n;j++) {
				x = ab[j][i-1];
				z = ab[j][i];
				ab[j][i-1] = x * c + z * s;
				ab[j][i] = -x * s + z * c;
			} /* end j */
			q[i-1] = z = sqrt(f*f + h*h);
			c = f/z;
			s = h/z;
			f = c * g + s * y;
			x = -s * g + c * y;
			for (j=n;j<np;j++) {
				y = ab[i-1][j];
				z = ab[i][j];
				ab[i-1][j] = y * c + z * s;
				ab[i][j] = -y * s + z * c;
			} /* end j */
		} /* end i */
		e[l] = 0.0;
		e[k] = f;
		q[k] = x;
		goto test_f_splitting;
convergence:
		if (z < 0.0) {
/* q[k] is made non-negative */
			q[k] = - z;
			for (j=0;j<n;j++)
				ab[j][k] = -ab[j][k];
		} /* end z */
	} /* end k */
	
	free(e);
}