// -*- c++ -*-

// Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk)

//All rights reserved.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions
//are met:
//1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//2. Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
//THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND OTHER CONTRIBUTORS ``AS IS''
//AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
//IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
//ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR OTHER CONTRIBUTORS BE
//LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
//CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
//SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
//INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
//CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.

#ifndef __SVD_H
#define __SVD_H

#include <TooN/TooN.h>
#include <TooN/lapack.h>

namespace TooN {

	// TODO - should this depend on precision?
static const double condition_no=1e9; // GK HACK TO GLOBAL







/**
@class SVD TooN/SVD.h
Performs %SVD and back substitute to solve equations.
Singular value decompositions are more robust than LU decompositions in the face of 
singular or nearly singular matrices. They decompose a matrix (of any shape) \f$M\f$ into:
\f[M = U \times D \times V^T\f]
where \f$D\f$ is a diagonal matrix of positive numbers whose dimension is the minimum 
of the dimensions of \f$M\f$. If \f$M\f$ is tall and thin (more rows than columns) 
then \f$U\f$ has the same shape as \f$M\f$ and \f$V\f$ is square (vice-versa if \f$M\f$ 
is short and fat). The columns of \f$U\f$ and the rows of \f$V\f$ are orthogonal 
and of unit norm (so one of them lies in SO(N)). The inverse of \f$M\f$ (or pseudo-inverse 
if \f$M\f$ is not square) is then given by
\f[M^{\dagger} = V \times D^{-1} \times U^T\f]
 
If \f$M\f$ is nearly singular then the diagonal matrix \f$D\f$ has some small values 
(relative to its largest value) and these terms dominate \f$D^{-1}\f$. To deal with 
this problem, the inverse is conditioned by setting a maximum ratio 
between the largest and smallest values in \f$D\f$ (passed as the <code>condition</code>
parameter to the various functions). Any values which are too small 
are set to zero in the inverse (rather than a large number)
 
It can be used as follows to solve the \f$M\underline{x} = \underline{c}\f$ problem as follows:
@code
// construct M
Matrix<3> M;
M[0] = makeVector(1,2,3);
M[1] = makeVector(4,5,6);
M[2] = makeVector(7,8.10);
// construct c
 Vector<3> c;
c = 2,3,4;
// create the SVD decomposition of M
SVD<3> svdM(M);
// compute x = M^-1 * c
Vector<3> x = svdM.backsub(c);
 @endcode

SVD<> (= SVD<-1>) can be used to create an SVD whose size is determined at run-time.
@ingroup gDecomps
**/
template<int Rows=Dynamic, int Cols=Rows, typename Precision=DefaultPrecision>
class SVD {
	// this is the size of the diagonal
	// NB works for semi-dynamic sizes because -1 < +ve ints
	static const int Min_Dim = Rows<Cols?Rows:Cols;
	
public:

	/// default constructor for Rows>0 and Cols>0
	SVD() {}

	/// constructor for Rows=-1 or Cols=-1 (or both)
	SVD(int rows, int cols)
		: my_copy(rows,cols),
		  my_diagonal(std::min(rows,cols)),
		  my_square(std::min(rows,cols), std::min(rows,cols))
	{}

	/// Construct the %SVD decomposition of a matrix. This initialises the class, and
	/// performs the decomposition immediately.
	template <int R2, int C2, typename P2, typename B2>
	SVD(const Matrix<R2,C2,P2,B2>& m)
		: my_copy(m),
		  my_diagonal(std::min(m.num_rows(),m.num_cols())),
		  my_square(std::min(m.num_rows(),m.num_cols()),std::min(m.num_rows(),m.num_cols()))
	{
		do_compute();
	}

	/// Compute the %SVD decomposition of M, typically used after the default constructor
	template <int R2, int C2, typename P2, typename B2>
	void compute(const Matrix<R2,C2,P2,B2>& m){
		my_copy=m;
		do_compute();
	}
	
	private:
	void do_compute(){
		Precision* const a = my_copy.my_data;
		int lda = my_copy.num_cols();
		int m = my_copy.num_cols();
		int n = my_copy.num_rows();
		Precision* const uorvt = my_square.my_data;
		Precision* const s = my_diagonal.my_data;
		int ldu;
		int ldvt = lda;
		int LWORK;
		int INFO;
		char JOBU;
		char JOBVT;

		if(is_vertical()){ // u is a
			JOBU='O';
			JOBVT='S';
			ldu = lda;
		} else { // vt is a
			JOBU='S';
			JOBVT='O';
			ldu = my_square.num_cols();
		}

		Precision* wk;

		Precision size;
		LWORK = -1;

		// arguments are scrambled because we use rowmajor and lapack uses colmajor
		// thus u and vt play each other's roles.
		gesvd_( &JOBVT, &JOBU, &m, &n, a, &lda, s, uorvt,
				 &ldvt, uorvt, &ldu, &size, &LWORK, &INFO);
	
		LWORK = (long int)(size);
		wk = new Precision[LWORK];

		gesvd_( &JOBVT, &JOBU, &m, &n, a, &lda, s, uorvt,
				 &ldvt, uorvt, &ldu, wk, &LWORK, &INFO);
	
		delete[] wk;
	}
	
	bool is_vertical(){ 
		return (my_copy.num_rows() >= my_copy.num_cols()); 
	}

	int min_dim(){ return std::min(my_copy.num_rows(), my_copy.num_cols()); }
	
	public:

	/// Calculate result of multiplying the (pseudo-)inverse of M by another matrix. 
	/// For a matrix \f$A\f$, this calculates \f$M^{\dagger}A\f$ by back substitution 
	/// (i.e. without explictly calculating the (pseudo-)inverse). 
	/// See the detailed description for a description of condition variables.
	template <int Rows2, int Cols2, typename P2, typename B2>
	Matrix<Cols,Cols2, typename Internal::MultiplyType<Precision,P2>::type >
	backsub(const Matrix<Rows2,Cols2,P2,B2>& rhs, const Precision condition=condition_no)
	{
		Vector<Min_Dim> inv_diag(min_dim());
		get_inv_diag(inv_diag,condition);
		return (get_VT().T() * diagmult(inv_diag, (get_U().T() * rhs)));
	}

	/// Calculate result of multiplying the (pseudo-)inverse of M by a vector. 
	/// For a vector \f$b\f$, this calculates \f$M^{\dagger}b\f$ by back substitution 
	/// (i.e. without explictly calculating the (pseudo-)inverse). 
	/// See the detailed description for a description of condition variables.
	template <int Size, typename P2, typename B2>
	Vector<Cols, typename Internal::MultiplyType<Precision,P2>::type >
	backsub(const Vector<Size,P2,B2>& rhs, const Precision condition=condition_no)
	{
		Vector<Min_Dim> inv_diag(min_dim());
		get_inv_diag(inv_diag,condition);
		return (get_VT().T() * diagmult(inv_diag, (get_U().T() * rhs)));
	}

	/// Calculate (pseudo-)inverse of the matrix. This is not usually needed: 
	/// if you need the inverse just to multiply it by a matrix or a vector, use 
	/// one of the backsub() functions, which will be faster.
	/// See the detailed description of the pseudo-inverse and condition variables.
	Matrix<Cols,Rows> get_pinv(const Precision condition = condition_no){
		Vector<Min_Dim> inv_diag(min_dim());
		get_inv_diag(inv_diag,condition);
		return diagmult(get_VT().T(),inv_diag) * get_U().T();
	}

	/// Calculate the product of the singular values
	/// for square matrices this is the determinant
	Precision determinant() {
		Precision result = my_diagonal[0];
		for(int i=1; i<my_diagonal.size(); i++){
			result *= my_diagonal[i];
		}
		return result;
	}
	
	/// Calculate the rank of the matrix.
	/// See the detailed description of the pseudo-inverse and condition variables.
	int rank(const Precision condition = condition_no) {
		if (my_diagonal[0] == 0) return 0;
		int result=1;
		for(int i=0; i<min_dim(); i++){
			if(my_diagonal[i] * condition <= my_diagonal[0]){
				result++;
			}
		}
		return result;
	}

	/// Return the U matrix from the decomposition
	/// The size of this depends on the shape of the original matrix
	/// it is square if the original matrix is wide or tall if the original matrix is tall
	Matrix<Rows,Min_Dim,Precision,Reference::RowMajor> get_U(){
		if(is_vertical()){
			return Matrix<Rows,Min_Dim,Precision,Reference::RowMajor>
				(my_copy.my_data,my_copy.num_rows(),my_copy.num_cols());
		} else {
			return Matrix<Rows,Min_Dim,Precision,Reference::RowMajor>
				(my_square.my_data, my_square.num_rows(), my_square.num_cols());
		}
	}

	/// Return the singular values as a vector
	Vector<Min_Dim,Precision>& get_diagonal(){ return my_diagonal; }

	/// Return the VT matrix from the decomposition
	/// The size of this depends on the shape of the original matrix
	/// it is square if the original matrix is tall or wide if the original matrix is wide
	Matrix<Min_Dim,Cols,Precision,Reference::RowMajor> get_VT(){
		if(is_vertical()){
			return Matrix<Min_Dim,Cols,Precision,Reference::RowMajor>
				(my_square.my_data, my_square.num_rows(), my_square.num_cols());
		} else {
			return Matrix<Min_Dim,Cols,Precision,Reference::RowMajor>
				(my_copy.my_data,my_copy.num_rows(),my_copy.num_cols());
		}
	}

	///Return the pesudo-inverse diagonal. The reciprocal of the diagonal elements
	///is returned if the elements are well scaled with respect to the largest element,
	///otherwise 0 is returned.
	///@param inv_diag Vector in which to return the inverse diagonal.
	///@param condition Elements must be larger than this factor times the largest diagonal element to be considered well scaled. 
	void get_inv_diag(Vector<Min_Dim>& inv_diag, const Precision condition){
		for(int i=0; i<min_dim(); i++){
			if(my_diagonal[i] * condition <= my_diagonal[0]){
				inv_diag[i]=0;
			} else {
				inv_diag[i]=static_cast<Precision>(1)/my_diagonal[i];
			}
		}
	}

private:
	Matrix<Rows,Cols,Precision,RowMajor> my_copy;
	Vector<Min_Dim,Precision> my_diagonal;
	Matrix<Min_Dim,Min_Dim,Precision,RowMajor> my_square; // square matrix (U or V' depending on the shape of my_copy)
};






/// version of SVD forced to be square
/// princiapally here to allow use in WLS
/// @ingroup gDecomps
template<int Size, typename Precision>
struct SQSVD : public SVD<Size, Size, Precision> {
	///All constructors are forwarded to SVD in a straightforward manner.
	///@name Constructors
	///@{
	SQSVD() {}
	SQSVD(int size) : SVD<Size,Size,Precision>(size, size) {}
	
	template <int R2, int C2, typename P2, typename B2>
	SQSVD(const Matrix<R2,C2,P2,B2>& m) : SVD<Size,Size,Precision>(m) {}
	///@}
};


}


#endif
