#include <math.h>
#include <stdio.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include "dalloca.h"
#include "head.h"

#define TOL 1.0e-20

#define ERRR (-1)
#ifndef NULL
#define NULL 0
#endif

static int svdcmp(double **a, int m, int n, double *w, double **v);
static int svbksb(double **u, double *w, double **v, int m, int n, double *b, double *x);

int Ft_svdfit(double **coef, double *y, double *sig, int ndata, double *a, int ma, double **v, double *w, double *chisq)
{
	int j,i;
	double wmax,tmp,thresh,sum,**u,*b;

	ADVECTOR(b, 1, ndata, "svdfit", Ft_catcher);
	ADMATRIX(u, 1, ndata, 1, ma, "svdfit", Ft_catcher);

	for (i=1;i<=ndata;i++) {
		tmp=1.0/sig[i];
		for (j=1;j<=ma;j++) u[i][j]=coef[j][i]*tmp;
		b[i]=y[i]*tmp;
	}
	if (svdcmp(u,ndata,ma,w,v) == ERRR) {
		Ft_catcher(ERRR);
	}
	wmax=0.0;
	for (j=1;j<=ma;j++)
		if (w[j] > wmax) wmax=w[j];
	thresh=TOL*wmax;
	for (j=1;j<=ma;j++)
		if (w[j] < thresh) w[j]=0.0;
	if (svbksb(u,w,v,ndata,ma,b,a) == ERRR) {
		Ft_catcher(ERRR);
	}
	*chisq=0.0;
	for (i=1;i<=ndata;i++) {
		for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*coef[j][i];
		*chisq += (tmp=(y[i]-sum)/sig[i],tmp*tmp);
	}
	return(1);
}

#undef TOL

static double at,bt,ct;
#define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))

static double maxarg1,maxarg2;
#define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

static int svdcmp(double **a, int m, int n, double *w, double **v)
{
	int flag, i, its, j, jj, k, l, nm;
	double c, f, h, s, x, y, z;
	double anorm=0.0, g=0.0;
	double scale=0.0;
	double *rv1;

	if (m < n) {
		fprintf(stderr, "svdcmp: You must augment A with extra zero rows.");
		return(ERRR);
	}
	ADVECTOR(rv1, 1, n, "svdcmp", return);

	l = nm = 0;
	for (i=1;i<=n;i++) {
		l = i+1;
		rv1[i] = scale*g;
		g = s = scale = 0.0;
		if (i <= m) {
			for (k=i;k<=m;k++)
				scale += fabs(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -SIGN(sqrt(s),f);
				h = f*g-s;
				a[i][i] = f-g;
				if (i != n) {
					for (j=l;j<=n;j++) {
						for (s=0.0,k=i;k<=m;k++)
							s += a[k][i]*a[k][j];
						f = s/h;
						for (k=i;k<=m;k++)
							a[k][j] += f*a[k][i];
					}
				}
				for (k=i;k<=m;k++)
					a[k][i] *= scale;
			}
		}
		w[i] = scale*g;
		g = s = scale = 0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++)
				scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f = a[i][l];
				g = -SIGN(sqrt(s),f);
				h = f*g-s;
				a[i][l] = f-g;
				for (k=l;k<=n;k++)
					rv1[k]=a[i][k]/h;
				if (i != m) {
					for (j=l;j<=m;j++) {
						for (s=0.0,k=l;k<=n;k++)
							s += a[j][k]*a[i][k];
						for (k=l;k<=n;k++)
							a[j][k] += s*rv1[k];
					}
				}
				for (k=l;k<=n;k++)
					a[i][k] *= scale;
			}
		}
		anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++)
					v[j][i] = (a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++)
						s += a[i][k]*v[k][j];
					for (k=l;k<=n;k++)
						v[k][j] += s*v[k][i];
				}
			}
			for (j=l;j<=n;j++)
				v[i][j]=v[j][i]=0.0;
		}
		v[i][i] = 1.0;
		g = rv1[i];
		l = i;
	}
	for (i=n;i>=1;i--) {
		l = i+1;
		g = w[i];
		if (i < n)
			for (j=l;j<=n;j++)
				a[i][j] = 0.0;
		if (g) {
			g = 1.0/g;
			if (i != n) {
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=m;k++)
						s += a[k][i]*a[k][j];
					f = (s/a[i][i])*g;
					for (k=i;k<=m;k++)
						a[k][j] += f*a[k][i];
				}
			}
			for (j=i;j<=m;j++)
				a[j][i] *= g;
		} else {
			for (j=i;j<=m;j++)
				a[j][i]=0.0;
		}
		++a[i][i];
	}
	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag = 1;
			for (l=k;l>=1;l--) {
				nm = l-1;
				if (fabs(rv1[l])+anorm == anorm) {
					flag=0;
					break;
				}
				if (fabs(w[nm])+anorm == anorm) break;
			}
			if (flag) {
				c = 0.0;
				s = 1.0;
				for (i=l;i<=k;i++) {
					f=s*rv1[i];
					if (fabs(f)+anorm != anorm) {
						g = w[i];
						h = PYTHAG(f,g);
						w[i]=h;
						h=1.0/h;
						c=g*h;
						s=(-f*h);
						for (j=1;j<=m;j++) {
							y=a[j][nm];
							z=a[j][i];
							a[j][nm]=y*c+z*s;
							a[j][i]=z*c-y*s;
						}
					}
				}
			}
			z=w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					for (j=1;j<=n;j++)
						v[j][k]=(-v[j][k]);
				}
				break;
			}
			if (its == 30) {
				fprintf(stderr, "svdcmp: No convergence in 30 iterations.\n");
				return(ERRR);
			}
			x=w[l];
			nm=k-1;
			y=w[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=PYTHAG(f,1.0);
			f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1.0;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=rv1[i];
				y=w[i];
				h=s*g;
				g=c*g;
				z=PYTHAG(f,h);
				rv1[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g=g*c-x*s;
				h=y*s;
				y=y*c;
				for (jj=1;jj<=n;jj++) {
					x=v[jj][j];
					z=v[jj][i];
					v[jj][j]=x*c+z*s;
					v[jj][i]=z*c-x*s;
				}
				z=PYTHAG(f,h);
				w[j]=z;
				if (z) {
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=(c*g)+(s*y);
				x=(c*y)-(s*g);
				for (jj=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	return(1);
}

#undef SIGN
#undef MAX
#undef PYTHAG

static int svbksb(double **u, double *w, double **v, int m, int n, double *b, double *x)
{
	int jj, j, i;
	double s, *tmp;

	ADVECTOR(tmp, 1, n, "svbksb", return);

	for (j=1;j<=n;j++) {
		s=0.0;
		if (w[j]) {
			for (i=1;i<=m;i++) s += u[i][j]*b[i];
			s /= w[j];
		}
		tmp[j]=s;
	}
	for (j=1;j<=n;j++) {
		s=0.0;
		for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
		x[j]=s;
	}
	return(1);
}

int Ft_svdvar(double **v, int ma, double *w, double **cvm)
{
	int k,j,i;
	double sum, *wti;

	ADVECTOR(wti, 1, ma, "svdvar", Ft_catcher);

	for (i=1;i<=ma;i++) {
		wti[i]=0.0;
		if (w[i]) wti[i]=1.0/(w[i]*w[i]);
	}
	for (i=1;i<=ma;i++) {
		for (j=1;j<=i;j++) {
			for (sum=0.0,k=1;k<=ma;k++) sum += v[i][k]*v[j][k]*wti[k];
			cvm[j][i]=cvm[i][j]=sum;
		}
	}
	return(0);
}
