/***************************************************************************
* SVD.C                                                                    *
*                                                                          *
* Singular Value Decomposition.                                            *
*                                                                          *
*   HISTORY                                                                *
*      Name    Date          Description                                   *
*                                                                          *
*      arvo    08/22/2000    Copied to CIT library.                        *
*      arvo    06/28/1993    Rewritten from "Numerical Recipes" C-code.    *
*                                                                          *
*--------------------------------------------------------------------------*
* Copyright (C) 2000, James Arvo                                           *
*                                                                          *
* This program is free software; you can redistribute it and/or modify it  *
* under the terms of the GNU General Public License as published by the    *
* Free Software Foundation.  See http://www.fsf.org/copyleft/gpl.html      *
*                                                                          *
* This program is distributed in the hope that it will be useful, but      *
* WITHOUT EXPRESS OR IMPLIED WARRANTY of merchantability or fitness for    *
* any particular purpose.  See the GNU General Public License for more     *
* details.                                                                 *
*                                                                          *
***************************************************************************/
#include <math.h>
#include <assert.h>
#include "ArvoMath.h"
#include "Vector.h"
#include "Matrix.h"
#include "SVD.h"

namespace ArvoMath {
	static const int MaxIterations = 30;

	static double svd_pythag( double a, double b )
	{
		double at = Abs(a);
		double bt = Abs(b);
		if( at > bt )
			return at * sqrt( 1.0 + Sqr( bt / at ) );
		else if( bt > 0.0 )
			return bt * sqrt( 1.0 + Sqr( at / bt ) );
		else return 0.0;
	}

	static inline double SameSign( double a, double b ) 
	{
		double t;
		if( b >= 0.0 ) t = Abs( a );
		else t = -Abs( a );
		return t;
	}

	static int ComputeRank( const Matrix &D, double epsilon )
	{
		int rank = 0;
		for( int i = 0; i < D.Rows(); i++ )
			if( Abs(D(i,i)) > epsilon ) rank++;
		return rank;
	}

	SVD::SVD( ) : Q_(0), D_(0), R_(0)
	{
	}

	SVD::SVD( const Matrix &M ) : Q_(0), D_(0), R_(0)
	{
		(*this) = M;
	}

	void SVD::operator=( const Matrix &A )
	{
		if( A.Rows() >= A.Cols() ) Q_ = A;
		else
		{
			Q_ = Matrix( A.Cols() );
			for( int i = 0; i < A.Rows(); i++ )
				for( int j = 0; j < A.Cols(); j++ ) Q_(i,j) = A(i,j);
		}
		R_ = Matrix( A.Cols() );
		Decompose( Q_, D_, R_ );
	}

	const Matrix &SVD::Q( double epsilon ) const
	{
		int rank = 0;
		if( epsilon != 0.0 ) rank = ComputeRank( D_, epsilon );
		return Q_;
	}

	const Matrix &SVD::D( double epsilon ) const
	{
		int rank = 0;
		if( epsilon != 0.0 ) rank = ComputeRank( D_, epsilon );
		return D_;
	}

	const Matrix &SVD::R( double epsilon ) const
	{
		int rank = 0;
		if( epsilon != 0.0 ) rank = ComputeRank( D_, epsilon );
		return R_;
	}

	int SVD::Rank( double epsilon ) const
	{
		return ComputeRank( D_, epsilon );
	}

	int SVD::Decompose( Matrix &Q, Matrix &D, Matrix &R )
	{
		int    i, j, k, l, m, n, p, q, iter;
		double c, f, h, s, x, y, z;
		double norm  = 0.0;
		double g     = 0.0;
		double scale = 0.0;

		m = Q.Rows();
		n = Q.Cols();

		Vector Temp( n );
		Vector diag( n );

		for( i = 0; i < n; i++ ) 
		{

			Temp(i) = scale * g;
			scale   = 0.0;
			g       = 0.0;
			s       = 0.0;
			l       = i + 1;

			if( i < m )
			{
				for( k = i; k < m; k++ ) scale += Abs( Q(k,i) );
				if( scale != 0.0 ) 
				{
					for( k = i; k < m; k++ ) 
					{
						Q(k,i) /= scale;
						s += Sqr( Q(k,i) );
					}
					f = Q(i,i);
					g = -SameSign( sqrt(s), f );
					h = f * g - s;
					Q(i,i) = f - g;
					if( i != n - 1 )
					{
						for( j = l; j < n; j++ ) 
						{
							s = 0.0;
							for( k = i; k < m; k++ ) s += Q(k,i) * Q(k,j);
							f = s / h;
							for( k = i; k < m; k++ ) Q(k,j) += f * Q(k,i);
						}
					}
					for( k = i; k < m; k++ ) Q(k,i) *= scale;
				}
			}

			diag(i) = scale * g;
			g       = 0.0;
			s       = 0.0;
			scale   = 0.0;

			if( i < m && i != n - 1 ) 
			{
				for( k = l; k < n; k++ ) scale += Abs( Q(i,k) );
				if( scale != 0.0 ) 
				{
					for( k = l; k < n; k++ ) 
					{
						Q(i,k) /= scale;
						s += Sqr( Q(i,k) );
					}
					f = Q(i,l);
					g = -SameSign( sqrt(s), f );
					h = f * g - s;
					Q(i,l) = f - g;
					for( k = l; k < n; k++ ) Temp(k) = Q(i,k) / h;
					if( i != m - 1 ) 
					{
						for( j = l; j < m; j++ ) 
						{
							s = 0.0;
							for( k = l; k < n; k++ ) s += Q(j,k) * Q(i,k);
							for( k = l; k < n; k++ ) Q(j,k) += s * Temp(k);
						}
					}
					for( k = l; k < n; k++ ) Q(i,k) *= scale;
				}
			}
			norm = Max( norm, Abs( diag(i) ) + Abs( Temp(i) ) );
		}


		for( i = n - 1; i >= 0; i-- ) 
		{
			if( i < n - 1 ) 
			{
				if( g != 0.0 ) 
				{
					for( j = l; j < n; j++ ) R(i,j) = ( Q(i,j) / Q(i,l) ) / g;
					for( j = l; j < n; j++ ) 
					{
						s = 0.0;
						for( k = l; k < n; k++ ) s += Q(i,k) * R(j,k);
						for( k = l; k < n; k++ ) R(j,k) += s * R(i,k);
					}
				}
				for( j = l; j < n; j++ ) 
				{
					R(i,j) = 0.0;
					R(j,i) = 0.0;
				}
			}
			R(i,i) = 1.0;
			g = Temp(i);
			l = i;
		}


		for( i = n - 1; i >= 0; i-- ) 
		{
			l = i + 1;
			g = diag(i);
			if( i < n - 1 ) for( j = l; j < n; j++ ) Q(i,j) = 0.0;
			if( g != 0.0 ) 
			{
				g = 1.0 / g;
				if( i != n - 1 ) 
				{
					for( j = l; j < n; j++ ) 
					{
						s = 0.0;
						for( k = l; k < m; k++ ) s += Q(k,i) * Q(k,j);
						f = ( s / Q(i,i) ) * g;
						for( k = i; k < m; k++ ) Q(k,j) += f * Q(k,i);
					}
				}
				for( j = i; j < m; j++ ) Q(j,i) *= g;
			} 
			else 
			{
				for( j = i; j < m; j++ ) Q(j,i) = 0.0;
			}
			Q(i,i) += 1.0;
		}


		for( k = n - 1; k >= 0; k-- ) 
		{
			for( iter = 1; iter <= MaxIterations; iter++ ) 
			{
				int jump;

				for( l = k; l >= 0; l-- )
				{
					q = l - 1;
					if( Abs( Temp(l) ) + norm == norm ) { jump = 1; break; }
					if( Abs( diag(q) ) + norm == norm ) { jump = 0; break; }
				}

				if( !jump )
				{
					c = 0.0;
					s = 1.0;
					for( i = l; i <= k; i++ )
					{
						f = s * Temp(i);
						Temp(i) *= c;
						if( Abs( f ) + norm == norm ) break;
						g = diag(i);
						h = svd_pythag( f, g );
						diag(i) = h;
						h = 1.0 / h;
						c = g * h;
						s = -f * h;
						for( j = 0; j < m; j++ ) 
						{
							y = Q(j,q);
							z = Q(j,i);
							Q(j,q) = y * c + z * s;
							Q(j,i) = z * c - y * s;
						}
					}
				}

				z = diag(k);
				if( l == k ) 
				{
					if( z < 0.0 ) 
					{
						diag(k) = -z;
						for( j = 0; j < n; j++ ) R(k,j) *= -1.0; 
					}
					break;
				}
				if( iter >= MaxIterations ) return 0;
				x = diag(l);
				q = k - 1;
				y = diag(q);
				g = Temp(q);
				h = Temp(k);
				f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0 * h * y );
				g = svd_pythag( f, 1.0 );
				f = ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + SameSign( g, f ) ) ) - h ) ) / x;
				c = 1.0;
				s = 1.0;
				for( j = l; j <= q; j++ ) 
				{
					i = j + 1;
					g = Temp(i);
					y = diag(i);
					h = s * g;
					g = c * g;
					z = svd_pythag( f, h );
					Temp(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( p = 0; p < n; p++ ) 
					{
						x = R(j,p);
						z = R(i,p);
						R(j,p) = x * c + z * s;
						R(i,p) = z * c - x * s;
					}
					z = svd_pythag( f, h );
					diag(j) = z;
					if( z != 0.0 ) 
					{
						z = 1.0 / z;
						c = f * z;
						s = h * z;
					}
					f = c * g + s * y;
					x = c * y - s * g;
					for( p = 0; p < m; p++ ) 
					{
						y = Q(p,j);
						z = Q(p,i);
						Q(p,j) = y * c + z * s;
						Q(p,i) = z * c - y * s;
					}
				}
				Temp(l) = 0.0;
				Temp(k) = f;
				diag(k) = x;
			}
		}

		// Sort the singular values into descending order.

		for( i = 0; i < n - 1; i++ )
		{
			double biggest = diag(i);  // Biggest singular value so far.
			int    bindex  = i;        // The row/col it occurred in.
			for( j = i + 1; j < n; j++ )
			{
				if( diag(j) > biggest ) 
				{
					biggest = diag(j);
					bindex  = j;
				}            
			}
			if( bindex != i )  // Need to swap rows and columns.
			{
				Q.SwapCols( i, bindex );  // Swap columns in Q.
				R.SwapRows( i, bindex );  // Swap rows in R.
				diag.Swap ( i, bindex );  // Swap elements in diag.
			}
		}

		D = Diag( diag );
		return 1;
	}


	const Matrix &SVD::PseudoInverse( double epsilon )
	{
		if( Null(P_) )
		{
			Matrix D_Inverse( D_ );
			for( int i = 0; i < D_Inverse.Rows(); i++ )
			{
				if( Abs( D_Inverse(i,i) ) > epsilon )
					D_Inverse(i,i) = 1.0 / D_Inverse(i,i);
				else D_Inverse(i,i) = 0.0;
			}
			P_ = Q_ * D_Inverse * R_;
		}
		return P_;
	}
};