/**
 * $Id: svd.h,v 1.6 2003/05/01 19:52:30 sirdude Exp $
 */

#ifndef SVD_H

#define SVD_H

// Compute the Single Value Decomposition of an arbitrary matrix A
// That is compute the 3 matrices U,W,V with U column orthogonal (m,n) 
// ,W a diagonal matrix and V an orthogonal square matrix s.t. 
// A = U.W.Vt. From this decomposition it is trivial to compute the 
// inverse of A as Ainv = V.Winv.tranpose(U).
// work_space is a temporary vector used by this class to compute
// intermediate values during the computation of the SVD. This should
// be of length a.num_cols. This is not checked

#include "tntmath.h"

namespace TNT
{


template <class MaTRiX, class VecToR >
void SVD(MaTRiX &a, VecToR &w,  MaTRiX &v, VecToR &work_space) {

		int n = a.num_cols();
	int m = a.num_rows();

	int flag,i,its,j,jj,k,l(0),nm(0);
	typename MaTRiX::value_type c,f,h,x,y,z;
	typename MaTRiX::value_type anorm(0),g(0),scale(0);
    typename MaTRiX::value_type s(0);

    work_space.newsize(n);

	for (i=1;i<=n;i++) {
		l=i+1;
		work_space(i)=scale*g;

		g = (typename MaTRiX::value_type)0;

		s = (typename  MaTRiX::value_type)0;
        scale = (typename  MaTRiX::value_type)0;

		if (i <= m) {
			for (k=i;k<=m;k++) scale += TNT::abs(a(k,i));
			if (scale > (typename  MaTRiX::value_type)0) {
				for (k=i;k<=m;k++) {
					a(k,i) /= scale;
					s += a(k,i)*a(k,i);
				}
				f=a(i,i);
				g = -TNT::sign(sqrt(s),f);
				h=f*g-s;
				a(i,i)=f-g;
				if (i != n) {
					for (j=l;j<=n;j++) {
                        s = (typename  MaTRiX::value_type)0;
						for (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 = (typename  MaTRiX::value_type)0;
        s = (typename  MaTRiX::value_type)0;
        scale = (typename  MaTRiX::value_type)0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += TNT::abs(a(i,k));
			if (scale > (typename  MaTRiX::value_type)0) {
				for (k=l;k<=n;k++) {
					a(i,k) /= scale;
					s += a(i,k)*a(i,k);
				}
				f=a(i,l);
				g = -TNT::sign(sqrt(s),f);
				h=f*g-s;
				a(i,l)=f-g;
				for (k=l;k<=n;k++) work_space(k)=a(i,k)/h;
				if (i != m) {
					for (j=l;j<=m;j++) {
                        s = (typename  MaTRiX::value_type)0;
						for (k=l;k<=n;k++) s += a(j,k)*a(i,k);
						for (k=l;k<=n;k++) a(j,k) += s*work_space(k);
					}
				}
				for (k=l;k<=n;k++) a(i,k) *= scale;
			}
		}
		anorm=TNT::max(anorm,(TNT::abs(w(i))+TNT::abs(work_space(i))));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g != (typename  MaTRiX::value_type)0) {
				for (j=l;j<=n;j++)
					v(j,i)=(a(i,j)/a(i,l))/g;
				for (j=l;j<=n;j++) {
                    s = (typename  MaTRiX::value_type)0;
					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);
				}
			}
			for (j=l;j<=n;j++) v(i,j)=v(j,i)= (typename  MaTRiX::value_type)0;
		}
		v(i,i)= (typename  MaTRiX::value_type)1;
		g=work_space(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)= (typename  MaTRiX::value_type)0;
		}
		if (g !=  (typename  MaTRiX::value_type)0) {
			g= ((typename  MaTRiX::value_type)1)/g;
			if (i != n) {
				for (j=l;j<=n;j++) {
                    s =  (typename  MaTRiX::value_type)0;
					for (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)= (typename  MaTRiX::value_type)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 (TNT::abs(work_space(l))+anorm == anorm) {
					flag=0;
					break;
				}
				if (TNT::abs(w(nm))+anorm == anorm) break;
			}
			if (flag) {
				c= (typename  MaTRiX::value_type)0;
				s= (typename  MaTRiX::value_type)1;
				for (i=l;i<=k;i++) {
					f=s*work_space(i);
					if (TNT::abs(f)+anorm != anorm) {
						g=w(i);
						h= (typename  MaTRiX::value_type)TNT::pythag(float(f),float(g));
						w(i)=h;
						h= ((typename  MaTRiX::value_type)1)/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 <  (typename  MaTRiX::value_type)0) {
					w(k) = -z;
					for (j=1;j<=n;j++) v(j,k)=(-v(j,k));
				}
				break;
			}


#if 1
			if (its == 30)
			{
                                TNTException an_exception;
                                an_exception.i = 0;
                                throw an_exception;

                                return ;
				assert(false);
			}
#endif
			x=w(l);
			nm=k-1;
			y=w(nm);
			g=work_space(nm);
			h=work_space(k);
			f=((y-z)*(y+z)+(g-h)*(g+h))/(((typename  MaTRiX::value_type)2)*h*y);
			g=(typename  MaTRiX::value_type)TNT::pythag(float(f), float(1));
			f=((x-z)*(x+z)+h*((y/(f+TNT::sign(g,f)))-h))/x;
                        c =  (typename  MaTRiX::value_type)1;
                        s =  (typename  MaTRiX::value_type)1;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=work_space(i);
				y=w(i);
				h=s*g;
				g=c*g;
				z=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
				work_space(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=(typename  MaTRiX::value_type)TNT::pythag(float(f),float(h));
				w(j)=z;
				if (z !=  (typename  MaTRiX::value_type)0) {
					z= ((typename  MaTRiX::value_type)1)/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;
				}
			}
			work_space(l)= (typename  MaTRiX::value_type)0;
			work_space(k)=f;
			w(k)=x;
		}
	}
}

// A is replaced by the column orthogonal matrix U 

template <class MaTRiX, class VecToR >
void SVD_a( MaTRiX &a, VecToR &w,  MaTRiX &v) {

	int n = a.num_cols();
	int m = a.num_rows();

	int flag,i,its,j,jj,k,l,nm;
	typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;

	VecToR work_space;
	work_space.newsize(n);

	g = scale = anorm = 0.0;
	
	for (i=1;i <=n;i++) {
		l = i+1;
		work_space(i) = scale*g;
		g = s=scale=0.0;

		if (i <= m) {
			for(k=i; k<=m; k++) scale += abs(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 += abs(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++) work_space(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*work_space(k);
				}
				for(k=l;k<=n;k++) a(i,k)*=scale;
			}
		}
		anorm = max(anorm,(abs(w(i)) + abs(work_space(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 = work_space(i);
		l = i;
	}

	for (i = min(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);
	}

	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag=1;
			for(l=k;l>=1;l--) {
				nm = l-1;
				if (abs(work_space(l)) + anorm == anorm) {
					flag = 0;
					break;
				}
				if (abs(w(nm)) + anorm == anorm) break;
			}
			if (flag) {
				c = 0.0;
				s = 1.0;
				for (i=l;i<=k;i++) {
					f = s*work_space(i);
					work_space(i) = c*work_space(i);
					if (abs(f) +anorm == anorm) break;
					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) assert(false);

			x=w(l);
			nm=k-1;
			y=w(nm);
			g=work_space(nm);
			h=work_space(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 = work_space(i);
				y=w(i);
				h=s*g;
				g=c*g;
				z=pythag(f,h);
				work_space(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=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;
				}
			}

			work_space(l) = 0.0;
			work_space(k) = f;
			w(k) = x;
		}
	}
}

}

#endif

