/*!
 *  \brief Computes the RQ-Decomposition of a matrix
 *
 *  \author  O.Krause
 *  \date    2012
 *
 * \par Copyright 1995-2015 Shark Development Team
 * 
 * <BR><HR>
 * This file is part of Shark.
 * <http://image.diku.dk/shark/>
 * 
 * Shark is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published 
 * by the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * Shark is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with Shark.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifndef SHARK_LINALG_PIVOTINGRQ_INL
#define SHARK_LINALG_PIVOTINGRQ_INL

#include <shark/LinAlg/rotations.h>
#include <shark/LinAlg/solveSystem.h>
#include <algorithm>

template<class MatrixT,class MatrixU>
std::size_t shark::blas::pivotingRQHouseholder
(
	matrix_expression<MatrixT> const& matrixA,
	matrix_container<MatrixU>& matrixR,
	matrix_container<MatrixU>& householderTransform,
	permutation_matrix& permutation
){
	std::size_t m = matrixA().size1();
	std::size_t n = matrixA().size2();
	std::size_t k = std::min(m,n);
	
	//initialize
	permutation.resize(m);
	matrixR() = matrixA;
	householderTransform().resize(k,n);
	householderTransform().clear();
	
	for(std::size_t i = 0; i != m; ++i)
		permutation(i) = i;
	
	//squared norm of the rows
	//in every step of computation, we choose the row with the highest norm
	RealVector norms(sum_columns(sqr(matrixA)));
	
	//threshold for rank determination. if the squared norm is lower than that
	//the matrix is considered to be 0.
	//shameless ripoff of the Eigen Library.
	//todo: this turned out to reveal false rnaks because the threshold is really
	//small. we use a bigger threshold here, until a rank revealing step is added
	//which is likely to be slow.
//	double threshold = *std::max_element(norms.begin(),norms.end());
//	threshold *= sqr(std::numeric_limits<double>::epsilon());//very small!
//	threshold /= n;
	double threshold = 1.e-15;
	
	//todo: make block oriented
	std::size_t rank = 0;
	for(std::size_t i = 0; i != k; ++i,++rank){
		//we work mainly on a subrange of R aside from pivoting
		matrix_range<MatrixU> subR = subrange(matrixR(),i,m,i,n);
		
		//step 1: pivoting
		std::size_t pivot = std::max_element(norms.begin()+i,norms.end())-norms.begin();
		//the values in the array are not numerically stable, so we have to recompute the pivot
		//before we can tell, whether we are done or not
		double pivotValue = norm_sqr(row(subR,pivot-i));
		//step1.1: rank analysis
		//test, whether we are done. there is no need in swapping if the remainder of the matrix is empty
		//todo: make numerically more sound
		if(pivotValue < threshold ){
			break;
		}
		//if the pivot does not equal the current i, we swap the matrix rows
		if(pivot != i){
			swap_rows(matrixR(),i,pivot);
			permutation(i) = pivot;
			std::swap(norms(i),norms(pivot));
		}
		
		//step 2: apply householder transformation
		//get the exact part of the current row which is used to store the householder reflection
		matrix_row<MatrixU> r = row(householderTransform(),i);
		vector_range<matrix_row<MatrixU> > reflection = subrange(r,i,n);
		
		//now we are sure that our current pivot is at index i and do a 
		//householder transformation on the first row
		double tau = createHouseholderReflection(row(subR,0),reflection);
		applyHouseholderOnTheRight(subR,reflection,tau);
		noalias(subrange(norms,i,m)) -= sqr(column(subR,0));	
	}
	
	//todo: This RQ algorithm is not really rank revealing, but according to the literature
	//"a good starting point". So there is still something to be done 
	
	//step3: fill the remainder of R with zeros
	subrange(matrixR(),rank,m,rank,n).clear();
	return rank;
}

template<class MatrixT,class Mat>
std::size_t shark::blas::pivotingRQ
(
	matrix_expression<MatrixT> const& matrixA,
	matrix_container<Mat>& matrixR,
	matrix_container<Mat>& matrixQ,
	permutation_matrix& permutation
){
	std::size_t n = matrixA().size2();
	
	//first we do the householder transformation to get R and the householder transformations
	Mat U;//householder matrix is called U in the paper
	std::size_t rank = pivotingRQHouseholder(matrixA, matrixR(), U,permutation);
	
	//having these, Q is quite efficient to compute using an algorithm from
	//T. Joffrain: On Accumulating Householder Transformations, 
	//ACM Transactions on Mathematical Software
	//The goal is to compute from the single householder transformations a matrix incorporating all of them
	//it turns out, that this is:
	//(I-u_1u_1^T)*...(I-u_lu_rank^T)= I-UT^-1U
	//with T = uppershark::blas::diag (U^TU) -1/2 diagof(U^T U)
	//this is of course for column major lower triangular matrices, 
	//meaning that we have to transpose our matrices U.
	Mat T = prod(rows(U,0,rank),trans(rows(U,0,rank)));
	//we now have to explicitely zero the lower half
	//and the diagonal needs to be divided by two.
	for(std::size_t i = 0; i != rank; ++i){
		for(std::size_t j = 0; j != i; ++j){
			T(i,j) = 0;
		}
		T(i,i) /= 2.0;
	}
	
	//Q is now I-UT^-1U T having full rank.
	//again for us it is U^T T^-1 U
	//we first solve for T^-1 U = Temp <=> T Temp = U
	//this saves us from computing the inverse of T
	Mat InvTU(rows(U,0,rank));
	solveTriangularSystemInPlace<SolveAXB,lower>(trans(T), InvTU);
	matrixQ().resize(n,n);
	//now Compute U^T temp = U^T T^-1 U
	noalias(matrixQ) = -prod(trans(rows(U,0,rank)),InvTU);
	shark::blas::diag (matrixQ) += 1;

	return rank;
}

#endif

