/* spqr.c, MATLAB Version 5

   The calling syntax is:

      [Qbar, R] = spqr (n, sigma)

   Urs von Matt, January 20, 1993
   Changed for Matlab V5 by Leonhard Jaschke, December 9, 1996
   Changed for Matlab V6 by strebel, August 30, 2001 */

#include <math.h>
#include "mex.h"
#include "blas.c"


static void QR (int n, double sigma, double *Qbar, mxArray *R)
{ int    i, j, k, nnz, n2, n3, n4;
  int    *ir, *jc;
  double *pr, t, tmp;

  /* initialize R */
  nnz = mxGetNzmax (R);
  n2 = n-2;
  n3 = n-3;
  n4 = n-4;
  ir = mxGetIr (R);
  jc = mxGetJc (R);
  pr = mxGetPr (R);
  /* diagonal of R */
  ir [0] = 0;
  for (i = 1; i < n2; i++) {ir [3*i - 1] = i;}
  /* first upper off-diagonal of R */
  for (i = 0; i < n3; i++) {ir [3*i + 1] = i;}
  /* second upper off-diagonal of R */
  for (i = 0; i < n4; i++) {ir [3*i + 3] = i;}
  /* columns of R */
  jc [0] = 0;
  jc [1] = 1;
  for (j=2; j < n2; j++) {jc [j] = 3*j - 3;}
  jc [n2] = nnz;

  /* compute the QR-decomposition */
#define r(i, j) pr [k = jc [j], k + i - ir [k]]
  r (0, 0) = 1.0;
  t = -2.0;
  if (n > 3) {r (1, 1) = 1.0;}
  for (j = 0; j < n2; j++) {
    tmp = sigma;
    rotg (&r (j, j), &tmp, &Qbar [j], &Qbar [n2 + j]);

    rotg (&r (j, j), &t, &Qbar [2*n2 + j], &Qbar [3*n2 + j]);
    if (j < n3) {
      r (j, j+1) = 0.0;
      rot (&r (j, j+1), &r (j+1, j+1), Qbar [2*n2 + j], Qbar [3*n2 + j]);
    }

    tmp = 1.0;
    rotg (&r (j, j), &tmp, &Qbar [4*n2 + j], &Qbar [5*n2 + j]);
    if (j < n3) {
      t = -2.0;
      rot (&r (j, j+1), &t, Qbar [4*n2 + j], Qbar [5*n2 + j]);
      if (j < n4) {
        r (j, j+2) = 0.0;
        r (j+2, j+2) = 1.0;
        rot (&r (j, j+2), &r (j+2, j+2), Qbar [4*n2 + j], Qbar [5*n2 + j]);
      }
    }
  }
#undef r
}



#define max(A, B)  ((A) > (B) ? (A) : (B))
#define min(A, B)  ((A) < (B) ? (A) : (B))

/* Input Arguments */
#define	n     prhs[0]
#define	sigma prhs[1]

/* Output Arguments */
#define	Qbar plhs[0]
#define	R    plhs[1]

void mexFunction (int nlhs, mxArray *plhs[],
		  int nrhs, const mxArray *prhs[])
{ int size, nnz;

  /* Check for proper number of arguments */
  if (nrhs != 2) {
    mexErrMsgTxt ("spqr requires two input arguments.");
  } else if (nlhs != 2) {
    mexErrMsgTxt ("spqr requires two output arguments.");
  }

  /* Check the dimensions of n. */
  if ((mxGetM (n) != 1) || (mxGetN (n) != 1) ||
      (*mxGetPr (n) < 3.0)) {
    mexErrMsgTxt ("n must be a scalar greater or equal 3.");
  }

  /* Check the dimensions of sigma. */
  if ((mxGetM (sigma) != 1) || (mxGetN (sigma) != 1)) {
    mexErrMsgTxt ("sigma must be a scalar.");
  }

  /* Create vectors for the return arguments. */
  size = (int) *mxGetPr (n);
  Qbar = mxCreateDoubleMatrix (size-2, 6, mxREAL);
  if (size == 3) {nnz = 1;} else {nnz = 3*size - 9;}
  R = mxCreateSparse (size-2, size-2, nnz, mxREAL);

  /* Do the actual computations in a subroutine. */
  QR (size, *mxGetPr (sigma), mxGetPr (Qbar), R);
}