/* QTx.c, MATLAB Version 5

   The calling syntax is:

      y = QTx (Qbar, x)

   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 QTx (int n, double *Qbar, double *x, double *y)
{ int j, n2;

  n2 = n-2;
  for (j = 0; j < n+n2; j++) {y [j] = x [j];}
  for (j = 0; j < n2; j++) {
    rot (&y[j], &y[n+j], Qbar [j], Qbar [n2 + j]);
    rot (&y[j], &y[j+1], Qbar [2*n2 + j], Qbar [3*n2 + j]);
    rot (&y[j], &y[j+2], Qbar [4*n2 + j], Qbar [5*n2 + j]);
  }
}



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

/* Input Arguments */
#define	Qbar prhs[0]
#define	x    prhs[1]

/* Output Arguments */
#define	y plhs[0]

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

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

  /* Check the dimensions of n. */
  n = mxGetM (Qbar) + 2;
  if ((n < 3) || (mxGetN (Qbar) != 6)) {
    mexErrMsgTxt ("Qbar must be a (n-2)-by-6 matrix, with n >= 3.");
  }

  /* Check the dimensions of x. */
  if ((mxGetM (x) != 2*n - 2) || (mxGetN (x) != 1)) {
    mexErrMsgTxt ("x must be a vector of size 2*n - 2, with n >= 3.");
  }

  /* Create vectors for the return arguments. */
  y = mxCreateDoubleMatrix (2*n - 2, 1, mxREAL);

  /* Do the actual computations in a subroutine. */
  QTx (n, mxGetPr (Qbar), mxGetPr (x), mxGetPr (y));
}