#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

/**
 * ** This is still largely untested  -- rlee@fpcc.net **
 *
 * Algorithm adapted from the GNU R-project's dqrdc2.f
 *
 * This QRPT decomp. uses householder transformations to compute the qr
 * factorization of an m by n matrix x.  A limited column
 * pivoting strategy, based on the 2-norms of the reduced columns,
 * moves columns with near-zero norm to the right-hand edge of
 * the x matrix.  This strategy means that sequential one
 * degree-of-freedom effects can be computed in a natural way.
 */

int
gslExt_linalg_QRPT_dqrdc2(gsl_matrix * A, 
                          gsl_vector * tau, 
                          gsl_permutation * p, 
                          int *signum, 
                          gsl_vector * norm, 
                          int *rank,
                          double tol)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      *signum = 1;

      /* Should check if rank == NULL first */
      *rank = N + 1;

      /* We need another norm vector to store the norms from the first iter. 
       * -- maybe should be provided by the client. 
       */
      gsl_vector *norm_work = gsl_vector_calloc(norm->size);

      gsl_permutation_init (p);	/* set to identity */

      /* Compute column norms and store in workspace */

      for (i = 0; i < N; i++)
	{
	  gsl_vector_view c = gsl_matrix_column (A, i);
	  double x = gsl_blas_dnrm2 (&c.vector);
	  gsl_vector_set (norm, i, x);
	}

      /* Store the original norms */
      for (i = 0; i < norm->size; i++) gsl_vector_set(norm_work, i, gsl_vector_get(norm, i));

      for (i = 0; i < GSL_MIN (M, N); i++)
	{
      size_t j;

      /**
       * Pivot the columns from left-to-right until one
       * with non-negligible norm is located.  
       * A column is considered to have become negligible 
       * if its norm has fallen below tol times its original norm.
       */
	  for (j = i; j < N; j++)
        {
	      double x_orig = gsl_vector_get (norm_work, j);
	      double x = gsl_vector_get (norm, j);

	      if ( x >= (x_orig*tol) ) break;
          else
            {
              gsl_matrix_swap_columns (A, j, N-1);
	          gsl_permutation_swap (p, j, N-1);
              gsl_vector_swap_elements (norm, j, N-1);
	          (*signum) = -(*signum);
              (*rank) = --(*rank);
            }
        }
      
	  {
	    gsl_vector_view c_full = gsl_matrix_column (A, i);
            gsl_vector_view c = gsl_vector_subvector (&c_full.vector, 
                                                      i, M - i);
	    double tau_i = gsl_linalg_householder_transform (&c.vector);

	    gsl_vector_set (tau, i, tau_i);

	    /* Apply the transformation to the remaining columns */

	    if (i + 1 < N)
	      {
		gsl_matrix_view m = gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i+1));

		gsl_linalg_householder_hm (tau_i, &c.vector, &m.matrix);
	      }
	  }

	  /* Update the norms of the remaining columns too */

          if (i + 1 < M) 
            {
              for (j = i + 1; j < N; j++)
                {
                  double y = 0;
                  double x = gsl_vector_get (norm, j);

		  if (x > 0.0)
		    {
		      double temp= gsl_matrix_get (A, i, j) / x;
                  
		      if (fabs (temp) >= 1)
			y = 0.0;
		      else
			y = y * sqrt (1 - temp * temp);
		      
		      /* recompute norm to prevent loss of accuracy */

		      if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
			{
			  gsl_vector_view c_full = gsl_matrix_column (A, j);
			  gsl_vector_view c = 
			    gsl_vector_subvector(&c_full.vector,
						 i+1, M - (i+1));
			  y = gsl_blas_dnrm2 (&c.vector);
			}
                  
		      gsl_vector_set (norm, j, y);
		    }
                }
            }
        }

      *rank = GSL_MIN(*rank, M);

      gsl_vector_free(norm_work);

      return GSL_SUCCESS;
    }
}