/* svd.h */
/* Singular Value Decomposition for solving linear algebraic equations */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/* prototype declarations */
void dsvdcmp(double **a, int m, int n, double w[], double **v);
double dpythag(double a, double b);
void dsvbksb(double **u, double w[], double **v, int m, int n, 
	    double b[], double x[]);


/* function definition */
void dsvdcmp(double **a, int m, int n, double w[], double **v)
     /* Given a matrix a[1..m][1..n], this routine computes
	its singular value decomposition, A = U W V^t.
	The matrix U replaces a on output. The diagonal matrix of 
	singular values W is output as a vector w[1..n]. 
	The matrix V (not the transpose V^t) is output as v[1..n][1..n]. */
{
  int flag, i, its, j, jj, k, l, nm;
  double anorm, c, f, g, h, s, scale, x, y, z, *rv1;

  rv1 = dvector(1, n);
  g = scale = anorm = 0.0;   /* Householder reduction to bidiagonal form. */
  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;
	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;
	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 = DMAX(anorm, (fabs(w[i]) + fabs(rv1[i])));
  }
  for (i = n; i >= 1; i--) { /* Accumulation of right-hand transformations. */
    if (i < n) {
      if (g) {
	/* Double division to avoid possible underflow */
	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;
  }
  /* Accumulation of left-hand transformations. */
  for (i = IMIN(m, n); i >= 1; i--) {
    l = i + 1;
    g = w[i];
    for (j = l; j <= n; j++) a[i][j] = 0.0;
    if (g) {
      g = 1.0/g;
      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];
  }
  /* Diagonalization of the bidiagonal form: Loop over singular values,
     and over allowed iterations. */
  for (k = n; k >= 1; k--) {
    for (its = 1; its <= 30; its++) {
      flag = 1;
      for (l = k; l >= 1; l--) {    /* Test for splitting. */
	nm = l - 1;                 /* Note that rv1[1] is always zero. */
	if ((double)(fabs(rv1[l]) + anorm) == anorm) {
	  flag = 0;
	  break;
	}
	if ((double)(fabs(w[nm]) + anorm) == anorm) break;
      }
      if (flag) {
	c = 0.0;     /* Cancellation of rv1[l], if l > 1. */
	s = 1.0;
	for (i = l; i <= k; i++) {
	  f = s*rv1[i];
	  rv1[i] = c*rv1[i];
	  if ((double)(fabs(f) + anorm) == anorm) break;
	  g = w[i];
	  h = dpythag(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) {     /* Convergence. */
	if (z < 0.0) {    /* Singular value is made nonnegative. */
	  w[k] = -z;
	  for (j = 1; j <= n; j++) v[j][k] = -v[j][k];
	}
	break;
      }
      if (its == 30) nrerror("no convergence in 30 dsvdcmp iterations");
      x = w[l];        /* Shift from bottom 2-by-2 minor. */
      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 = dpythag(f, 1.0);
      f = ((x - z)*(x + z) + h*((y/(f + SIGN(g, f))) - h))/x;
      c = s = 1.0;          /* Next QR transformation: */
      for (j = l; j <= nm; j++) {
	i = j + 1;
	g = rv1[i];
	y = w[i];
	h = s*g;
	g = c*g;
	z = dpythag(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 *= 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 = dpythag(f, h);
	w[j] = z;    /* Rotation can be arbitrary if z = 0. */
	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;
    }
  }
  free_dvector(rv1, 1, n);
}


double dpythag(double a, double b)
     /* Computes sqrt(a*a + b*b) without destructive underflow or overflow */
{
  double absa, absb;
  absa = fabs(a);
  absb = fabs(b);

  if (absa > absb) return absa*sqrt(1.0 + DSQR(absb/absa));
  else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0 + DSQR(absa/absb)));
}


void dsvbksb(double **u, double w[], double **v, int m, int n, 
	    double b[], double x[])
     /* Solves AX = B for a vector X, where A is specified by the arrays 
	u[1..m][1..n], w[1..n], v[1..n][1..n] as returned by svdcmp.
	m and n are the dimensions of A, and will be equal for
	square matrices. B[1..m] is the input right-hand side.
	X[1..n] is the output solution vector. No input quantities are
	destroyed, so the routine may be called sequentially with 
	different B's. */
{
  int jj, j, i;
  double s, *tmp;

  tmp = dvector(1, n);
  for (j = 1; j <= n; j++) {   /* calculate U^t B */
    s = 0.0;
    if (w[j]) {    /* nonzero result onlu if w_j is nonzero */
      for (i = 1; i <= m; i++) s += u[i][j] * b[i];
      s /= w[j];   /* this is the divide by w_j */
    }
    tmp[j] = s;
  }
  for (j = 1; j <= n; j++) {   /* matrix multiply by V to get answer */
    s = 0.0;
    for (jj = 1; jj <= n; jj++) s += v[j][jj]*tmp[jj];
    x[j] = s;
  }
  free_dvector(tmp, 1, n);
}