#include <math.h>
#include <limits.h>
#include <mylib.h>
#include <stdio.h>
#include <assert.h>

struct svdcmp_t { double *w; double **v;
		svdcmp_t(double *w_, double **v_): w(w_), v(v_) {}
		svdcmp_t(): w(0), v(0) {}
		};



inline double fmax(double a, double b)
{
 return a>b ? a : b;
}


inline int imin(int a, int b)
{
 return a<b ? a : b;
}


svdcmp_t svdcmp(double **a)
{
	C double R_TINY = sqrt(DBL_MIN);

	C int n = double2size(a).cols;
	if(n == 0)
		return svdcmp_t(0, 0);
	C int m = double2size(a).rows;
	if(m == 0)
		return svdcmp_t(0, 0);
	// Normally, m>n.  If m<n, then you guarentee that there is a nullspace of v,
	// and guarentee that n-m elements if w will be (near) zero.
	// Svd makes a into a matrix where the columns are orthogonal.

	double *w = double1alloc(n);
	double **v = double2alloc(n, n);
	assert(w && v);

	double *rv1 = double1alloc(n);
	assert(rv1);
	double g = 0.0, scale = 0.0, anorm = 0.0;
	int i;
	for (i=0; i<n; i++) {
		C int l = i+1;
		rv1[i] = scale*g;
		g = scale = 0.0;
		if (i < m) {
			int k;
			for (k=i; k<m; k++)
				scale += fabs(a[k][i]);
			if (scale > R_TINY) {
				double s = 0.0;
				for (k=i; k<m; k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				C double f = a[i][i];
				g = -copysign(sqrt(s), f);
				C double h = f*g - s;
				if(fabs(h) > R_TINY) {
					a[i][i] = f-g;
					int j;
					for (j=l; j<n; j++) {
						double ss = 0.0;
						int kk;
						for (kk=i; kk<m; kk++)
							ss += a[kk][i]*a[kk][j];
						C double ff = ss/h;
						for (kk=i; kk<m; kk++)
							a[kk][j] += ff*a[kk][i];
					}
				}
				for (k=i; k<m; k++)
					a[k][i] *= scale;	/* here */
			}
		}
		w[i] = scale * g;
		(g, scale) = (0.0, 0.0);
		if (i < m && i != n-1) {
			int k;
			for (k=l; k<n; k++)
				scale += fabs(a[i][k]);
			if (scale > R_TINY) {
				double s = 0.0;
				for (k=l; k<n; k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				C double f = a[i][l];
				g = -copysign(sqrt(s), f);
				C double h = f*g - s;
				if(fabs(h) > R_TINY)	{
					a[i][l] = f-g;
					for (k=l; k<n; k++)
						rv1[k] = a[i][k]/h;
					int j;
					for (j=l; j<m; j++) {
						double ss = 0.0;
						for (k=l; k<n; k++)
							ss += a[j][k]*a[i][k];
						for (k=l; k<n; k++)
							a[j][k] += ss*rv1[k];
					}
				}
				for (k=l; k<n; k++)
					a[i][k] *= scale;
			}
		}
		anorm = fmax(anorm, fabs(w[i])+fabs(rv1[i]));
	}

	int l = n;
	for (i=n-1; i>=0; i--) {
		if (i < n-1) {
			if (fabs(g) > R_TINY) {
				int j;
				for (j=l; j<n; j++)
					v[j][i] = (a[i][j]/a[i][l])/g;
				for (j=l; j<n; j++) {
					double s = 0.0;
					int k;
					for (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];
				}
			}
			int j;
			for (j=l; j<n; j++)	{
				v[j][i] = 0.0;
				v[i][j] = 0.0;
				}
		}
		v[i][i] = 1.0;
		g = rv1[i];
		l = i;
	}

	for (i=imin(m,n)-1; i>=0; i--) {
		l = i+1;
		int j;
		for (j=l; j<n; j++)
			a[i][j] = 0.0;
		if (fabs(w[i]) > R_TINY && fabs(a[i][i]) > R_TINY) {
			C double gg = 1.0/w[i];
			for (j=l; j<n; j++) {
				double s = 0.0;
				int k;
				for (k=l; k<m; k++)
					s += a[k][i]*a[k][j];
				C double f = (s/a[i][i])*gg;
				for (k=i; k<m; k++)
					a[k][j] += f*a[k][i];
			}
			for (j=i; j<m; j++)
				a[j][i] *= gg;
		} else for (j=i; j<m; j++)
				a[j][i] = 0.0;
		a[i][i] += 1.0;
	}

	int k;
	for (k=n-1; k>=0; k--) {
		int its;
		for (its=0; its<30; its++) {
			int flag = 1;
			int nm = -1;
			for (l=k; l>=0; l--) {
				nm = l-1;
				if (fabs(rv1[l])+anorm == anorm) {
					flag = 0;
					break;
				}
				if (fabs(w[nm])+anorm == anorm)
					break;
			}
			if (flag) {
				double c = 0.0, s = 1.0;
				for (i=l; i<=k; i++) {
					C double f = s*rv1[i];
					rv1[i] = c*rv1[i];
					if (fabs(f)+anorm == anorm)
						break;
					g = w[i];
					C double h = hypot(f, g);
					w[i] = h;
					if(fabs(h) > R_TINY)
						(c, s) = (g/h, -f/h);
					else	(c, s) = (1.0, 0.0);
					int j;
					for (j=0; j<m; j++) {
						C double y = a[j][nm];
						C double z = a[j][i];
						a[j][nm] = y*c+z*s;
						a[j][i] = z*c-y*s;
					}
				}
			}
			C double z = w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					int j;
					for (j=0; j<n; j++)
						v[j][k] = -v[j][k];
				}
				break;
			}
			if (its == 30)	{
				fprintf(stderr, "ERR: no convergence in 30 svdcmp iterations");
				return svdcmp_t();
				}
			C double x = w[l];
			nm = k-1;
			C double y = w[nm];
			g = rv1[nm];
			C double hh = rv1[k];
			C double ff = ((y-z)*(y+z)+(g-hh)*(g+hh)) / (2.0*hh*y);
			g = hypot(ff, 1.0);
			C double f = ((x-z)*(x+z) + hh*((y/(ff+copysign(g, ff)))-hh)) / x;
			double c = 1.0, s = 1.0;
			int j;
			for (j=l; j<=nm; j++) {
				i = j+1;
				C double yi = w[i];
				C double h = s * rv1[i];
				g = c * rv1[i];
				C double zz = hypot(f, h);
				rv1[j] = zz;
				if(zz > R_TINY)
					(c, s) = (f/zz, h/zz);
				else	(c, s) = (1.0, 0.0);
				f = x*c+g*s;
				g = g*c-x*s;
				h = yi*s;
				yi *= c;
				for (jj:=0; jj<n; jj++) {
					xj := v[jj][j];
					zj := v[jj][i];
					v[jj][j] = xj*c + zj*s;
					v[jj][i] = zj*c - xj*s;
				}
				z2 := hypot(f,h);
				w[j] = z2;
				if (z2 > R_TINY) {
					c = f/z2;
					s = h/z2;
				}
				f = c*g + s*yi;
				x = c*yi - s*g;
				for (jj=0; jj<m; jj++) {
					yj := a[jj][j];
					zj := a[jj][i];
					a[jj][j] = yj*c + zj*s;
					a[jj][i] = zj*c - yj*s;
				}
			}
			rv1[l] = 0.0;
			rv1[k] = f;
			w[k] = x;
		}
	}
 return (w, v);
}





int imin(int i, int j)
{
 if(i < j)
	return i;
 return j;
}





double ** recompose(double **u, C double *w: array of real, double **v)
{
 C int r = double2size(u).rows;
 C int c = double2size(v).cols;
 C int d = double1size(w);
 assert(d==double2size(u).cols && d==double2size(v).rows);
 double *rv = double2alloc(r, c);
 assert(rv);
 int i;
 for(i=0; i<r; i++)	{
	int j;
	for(j=0; j<c; j++)	{
		double s = 0.0;
		int k;
		for(k=0; k<d; k++)
			s += u[i][k] * w[k] * v[j][k];
		rv[i][j] = s;
		}
	}
 return rv;
}

