/* This function computes the solution to the least squares system   phi = [ A x =  b , lambda D x = 0 ]^2       where A is an M by N matrix, D is an N by N diagonal matrix, lambda   is a scalar parameter and b is a vector of length M.   The function requires the factorization of A into A = Q R P^T,   where Q is an orthogonal matrix, R is an upper triangular matrix   with diagonal elements of non-increasing magnitude and P is a   permuation matrix. The system above is then equivalent to   [ R z = Q^T b, P^T (lambda D) P z = 0 ]   where x = P z. If this system does not have full rank then a least   squares solution is obtained.  On output the function also provides   an upper triangular matrix S such that   P^T (A^T A + lambda^2 D^T D) P = S^T S   Parameters,      r: On input, contains the full upper triangle of R. On output the   strict lower triangle contains the transpose of the strict upper   triangle of S, and the diagonal of S is stored in sdiag.  The full   upper triangle of R is not modified.   p: the encoded form of the permutation matrix P. column j of P is   column p[j] of the identity matrix.   lambda, diag: contains the scalar lambda and the diagonal elements   of the matrix D   qtb: contains the product Q^T b   x: on output contains the least squares solution of the system   wa: is a workspace of length N   */static intqrsolv (gsl_matrix * r, const gsl_permutation * p, const double lambda,         const gsl_vector * diag, const gsl_vector * qtb,         gsl_vector * x, gsl_vector * sdiag, gsl_vector * wa){  size_t n = r->size2;  size_t i, j, k, nsing;  /* Copy r and qtb to preserve input and initialise s. In particular,     save the diagonal elements of r in x */  for (j = 0; j < n; j++)    {      double rjj = gsl_matrix_get (r, j, j);      double qtbj = gsl_vector_get (qtb, j);      for (i = j + 1; i < n; i++)        {          double rji = gsl_matrix_get (r, j, i);          gsl_matrix_set (r, i, j, rji);        }      gsl_vector_set (x, j, rjj);      gsl_vector_set (wa, j, qtbj);    }  /* Eliminate the diagonal matrix d using a Givens rotation */  for (j = 0; j < n; j++)    {      double qtbpj;      size_t pj = gsl_permutation_get (p, j);      double diagpj = lambda * gsl_vector_get (diag, pj);      if (diagpj == 0)        {          continue;        }      gsl_vector_set (sdiag, j, diagpj);      for (k = j + 1; k < n; k++)        {          gsl_vector_set (sdiag, k, 0.0);        }      /* The transformations to eliminate the row of d modify only a         single element of qtb beyond the first n, which is initially         zero */      qtbpj = 0;      for (k = j; k < n; k++)        {          /* Determine a Givens rotation which eliminates the             appropriate element in the current row of d */          double sine, cosine;          double wak = gsl_vector_get (wa, k);          double rkk = gsl_matrix_get (r, k, k);          double sdiagk = gsl_vector_get (sdiag, k);          if (sdiagk == 0)            {              continue;            }          if (fabs (rkk) < fabs (sdiagk))            {              double cotangent = rkk / sdiagk;              sine = 0.5 / sqrt (0.25 + 0.25 * cotangent * cotangent);              cosine = sine * cotangent;            }          else            {              double tangent = sdiagk / rkk;              cosine = 0.5 / sqrt (0.25 + 0.25 * tangent * tangent);              sine = cosine * tangent;            }          /* Compute the modified diagonal element of r and the             modified element of [qtb,0] */          {            double new_rkk = cosine * rkk + sine * sdiagk;            double new_wak = cosine * wak + sine * qtbpj;                        qtbpj = -sine * wak + cosine * qtbpj;            gsl_matrix_set(r, k, k, new_rkk);            gsl_vector_set(wa, k, new_wak);          }          /* Accumulate the transformation in the row of s */          for (i = k + 1; i < n; i++)            {              double rik = gsl_matrix_get (r, i, k);              double sdiagi = gsl_vector_get (sdiag, i);                            double new_rik = cosine * rik + sine * sdiagi;              double new_sdiagi = -sine * rik + cosine * sdiagi;                            gsl_matrix_set(r, i, k, new_rik);              gsl_vector_set(sdiag, i, new_sdiagi);            }        }      /* Store the corresponding diagonal element of s and restore the         corresponding diagonal element of r */      {        double rjj = gsl_matrix_get (r, j, j);        double xj = gsl_vector_get(x, j);                gsl_vector_set (sdiag, j, rjj);        gsl_matrix_set (r, j, j, xj);      }    }  /* Solve the triangular system for z. If the system is singular then     obtain a least squares solution */  nsing = n;  for (j = 0; j < n; j++)    {      double sdiagj = gsl_vector_get (sdiag, j);      if (sdiagj == 0)        {          nsing = j;          break;        }    }  for (j = nsing; j < n; j++)    {      gsl_vector_set (wa, j, 0.0);    }  for (k = 0; k < nsing; k++)    {      double sum = 0;      j = (nsing - 1) - k;      for (i = j + 1; i < nsing; i++)        {          sum += gsl_matrix_get(r, i, j) * gsl_vector_get(wa, i);        }      {        double waj = gsl_vector_get (wa, j);        double sdiagj = gsl_vector_get (sdiag, j);        gsl_vector_set (wa, j, (waj - sum) / sdiagj);      }    }  /* Permute the components of z back to the components of x */  for (j = 0; j < n; j++)    {      size_t pj = gsl_permutation_get (p, j);      double waj = gsl_vector_get (wa, j);      gsl_vector_set (x, pj, waj);    }  return GSL_SUCCESS;}