#include <math.h>
#include <stdio.h>
#ifndef NOUNISTD_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);
}