/* $Revision: 2.0 $ $Date: 3/11/03 */
/*******************************************************************
 * [G1,C,impact,fmat,fwt,ywt,gev,eu]=gensys(g0,g1,c,psi,pi,div)
 *
 * System given as
 *         g0*y(t)=g1*y(t-1)+c+psi*z(t)+pi*eta(t),
 * with z an exogenous variable process and eta being endogenously determined
 * one-step-ahead expectational errors.  Returned system is
 *        y(t)=G1*y(t-1)+C+impact*z(t)+ywt*inv(I-fmat*inv(L))*fwt*z(t+1) .
 * If z(t) is i.i.d., the last term drops out.
 * If stake is omitted from argument list, a stake>1 is calculated.
 * eu(1)=1 for existence, eu(2)=1 for uniqueness.  eu(1)=-1 for
 * existence only with not-s.c. z; eu=[-2,-2] for coincident zeros.
 *
 * MATLAB algorithm by Christopher Sims
 * C implementation by Iskander Karibzhanov
 * Master of Science in Computational Finance
 * Georgia Institute of Technology
 *
 * This is a MEX-file for MATLAB.
 * Copyright 1996 Christopher Sims
 * Copyright 2002 Federal Reserve Bank of Atlanta
 *******************************************************************/

#include <math.h>
#include "mex.h"
#include "mkl_types.h"
#include "mkl_cblas.h"
#include "mkl_lapack.h"

#define REALSMALL 1e-6

int qz(MKL_Complex16 *a, MKL_Complex16 *b, MKL_Complex16 *q, MKL_Complex16 *z, int n);
void copy_eigenvalues(mxArray *gev, MKL_Complex16 *a, MKL_Complex16 *b, int n);
int compute_svd(MKL_Complex16 *a, MKL_Complex16 **u, double **d, MKL_Complex16 **v, int m, int n);
int compute_norm(MKL_Complex16 *a, double **d, int m, int n);
int compute_normx(MKL_Complex16 *a, MKL_Complex16 *b, MKL_Complex16 *zwt, MKL_Complex16 *ueta, double **normx, int nunstab, int psin, int n, int bigev);
MKL_Complex16* mat2mkl(const mxArray *X, int ldz, int ndz);
mxArray* mkl2mat(MKL_Complex16 *Z, int ldz, int m, int n);
mxArray* mkl2mat_real(MKL_Complex16 *Z, int ldz, int m, int n);
void cblas_zdupe(int m, int n, MKL_Complex16 *a, int lda, MKL_Complex16 *b, int ldb);
void cblas_zdscali(int n, double *a, int lda, MKL_Complex16 *b, int ldb);
void cblas_zdscale(int n, double *a, int lda, MKL_Complex16 *b, int ldb);
int triles(int n, int nrhs, MKL_Complex16 *a, int lda, MKL_Complex16 *b, int ldb);
void cblas_zdpsb(int m, int n, MKL_Complex16 *a, int lda, MKL_Complex16 *b, int ldb, MKL_Complex16 *c, int ldc);

bool fixdiv = true, zxz = false;
double stake = 1.01;
int nunstab = 0;
MKL_Complex16 one, minusone, zero;

/* [G1,C,impact,fmat,fwt,ywt,gev,eu]=gensysmkl(g0,g1,c,psi,pi,stake) */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
   int n, psin, pin, md, mds, md1, i, bigev, bigevs, bigev1;
   double *eu, *deta, *dz, *norm, *normx, *deta1;
   MKL_Complex16 *a, *b, *q, *z, *pi, *psi, *tmat, *g0, *g1, *dummy, *tmatq, *ab, *c, *impact, *fmat, *fwt, *ywt;
   MKL_Complex16 *etawt, *ueta, *veta, *zwt, *uz, *vz, *etawt1, *ueta1, *veta1;
   bool exist = false, existx = false, unique = false;

   /* Check for proper number of input and output arguments */
   if (nrhs < 5 || nrhs > 6)
      mexErrMsgTxt("Five or six input arguments required.\n");
   if (nlhs != 8)
      mexErrMsgTxt("Eight output arguments required.\n");

   /* Dimensions */
   n = mxGetM(prhs[0]);
   psin = mxGetN(prhs[3]);
   pin = mxGetN(prhs[4]);

   /* Check for consistency of input matrix dimensions */
   if (mxGetM(prhs[0])!=n || mxGetM(prhs[1])!=n)
      mexErrMsgTxt("g0 and g1 must be square matrices.\n");

   if (mxGetM(prhs[3])!=n || mxGetM(prhs[4])!=n)
      mexErrMsgTxt("psi and pi must have the same number of rows as g0.\n");

   eu = mxGetPr(plhs[7]=mxCreateDoubleMatrix(2,1,mxREAL));

   /* [a b q z]=qz(g0,g1); */
   a = mat2mkl(prhs[0],n,n);
   b = mat2mkl(prhs[1],n,n);
   q = mxCalloc(n*n,sizeof(MKL_Complex16));
   z = mxCalloc(n*n,sizeof(MKL_Complex16));

   fixdiv = nrhs != 6;
   stake = fixdiv ? 1.01 : mxGetScalar(prhs[5]);
   nunstab = 0;
   zxz = false;

   if (qz(a, b, q, z, n)) {
      mexWarnMsgTxt("QZ factorization failed.\n");
      mxFree(a);
      mxFree(b);
      mxFree(q);
      mxFree(z);
      return;
   }

   nunstab /= 2;

/* mexPrintf("stake=%f\nnunstab=%d\n",stake,nunstab); */

   if (zxz) {
      mexWarnMsgTxt("Coincident zeros.  Indeterminacy and/or nonexistence.\n");
      eu[0] = eu[1] = -2;
      mxFree(a);
      mxFree(b);
      mxFree(q);
      mxFree(z);
      return;
   }

   plhs[6] = mxCreateDoubleMatrix(n, 2, mxCOMPLEX);
   copy_eigenvalues(plhs[6], a, b, n);

   one.real = 1.0;
   one.imag = 0.0;

   minusone.real = -1.0;
   minusone.imag = 0.0;

   zero.real = 0.0;
   zero.imag = 0.0;

   pi = mat2mkl(prhs[4],n,pin);
   etawt = mxCalloc(nunstab*pin,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasConjTrans, CblasNoTrans, nunstab, pin, n,
               &one, q+n*(n-nunstab), n, pi, n, &zero, etawt, nunstab);
   if (compute_svd(etawt, &ueta, &deta, &veta, nunstab, pin)) {
      mexWarnMsgTxt("SVD failed.\n");
      mxFree(ueta);
      mxFree(deta);
      mxFree(veta);
      mxFree(etawt);
      mxFree(a);
      mxFree(b);
      mxFree(q);
      mxFree(z);
      return;
   }
   mxFree(etawt);
   md = nunstab<pin?nunstab:pin;
   bigev = md;
   for (i=0; i<md; i++)
      if (deta[i]<=REALSMALL) {
         bigev=i;
         break;
      }

   psi = mat2mkl(prhs[3],n,psin);
   zwt = mxCalloc(nunstab*psin,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasConjTrans, CblasNoTrans, nunstab, psin, n,
               &one, q+n*(n-nunstab), n, psi, n, &zero, zwt, nunstab);
   if (compute_svd(zwt, &uz, &dz, &vz, nunstab, psin)) {
      mexWarnMsgTxt("SV decomposition failed.\n");
      mxFree(ueta);
      mxFree(deta);
      mxFree(veta);
      mxFree(uz);
      mxFree(dz);
      mxFree(vz);
      mxFree(zwt);
      mxFree(a);
      mxFree(b);
      mxFree(q);
      mxFree(z);
      return;
   }
   mxFree(vz);
   mds = nunstab<psin?nunstab:psin;
   bigevs = mds;
   for (i=0; i<mds; i++)
      if (dz[i]<=REALSMALL) {
         bigevs=i;
         break;
      }
   mxFree(dz);

   /* ueta = nunstab x bigev
      deta = bigev x 1
      veta = bigev x pin, ldveta = md
      uz = nunstab x bigevs
      dz = bigevs x 1
      vz = bigevs x psin, ldvz = mds */

   if (!bigevs) {
      exist = true;
      existx = true;
   } else {
      /* uz-ueta*ueta'*uz */
      MKL_Complex16 *tmp = mxCalloc(nunstab*nunstab,sizeof(MKL_Complex16));
      cblas_zgemm(CblasColMajor, CblasNoTrans, CblasConjTrans, nunstab, nunstab,
         bigev, &one, ueta, nunstab, ueta, nunstab, &zero, tmp, nunstab);
      cblas_zhemm(CblasColMajor, CblasLeft, CblasUpper, nunstab,
         bigevs, &minusone, tmp, nunstab, uz, nunstab, &one, uz, nunstab);
      mxFree(tmp);
      if (compute_norm(uz, &norm, nunstab, bigevs)) {
         mexWarnMsgTxt("SVD failed.\n");
         mxFree(norm);
         mxFree(ueta);
         mxFree(deta);
         mxFree(veta);
         mxFree(uz);
         mxFree(zwt);
         mxFree(a);
         mxFree(b);
         mxFree(q);
         mxFree(z);
         return;
      }
      exist = *norm < REALSMALL*n;
      mxFree(norm);
      if (compute_normx(a, b, zwt, ueta, &normx, nunstab, psin, n, bigev)) {
         mxFree(ueta);
         mxFree(deta);
         mxFree(veta);
         mxFree(uz);
         mxFree(zwt);
         mxFree(a);
         mxFree(b);
         mxFree(q);
         mxFree(z);
         return;
      }
      existx = *normx < REALSMALL*n;
      mxFree(normx);
   }

   mxFree(uz);
   mxFree(zwt);


/* Note that existence and uniqueness are not just matters of comparing
   numbers of roots and numbers of endogenous errors.  These counts are
   reported below because usually they point to the source of the problem. */

   etawt1 = mxCalloc((n-nunstab)*pin,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasConjTrans, CblasNoTrans, n-nunstab, pin, n,
      &one, q, n, pi, n, &zero, etawt1, n-nunstab);
   mxFree(pi);
   if (compute_svd(etawt1, &ueta1, &deta1, &veta1, n-nunstab, pin)) {
      mexWarnMsgTxt("SVD failed.\n");
      mxFree(ueta1);
      mxFree(deta1);
      mxFree(veta1);
      mxFree(etawt1);
      mxFree(ueta);
      mxFree(deta);
      mxFree(veta);
      mxFree(a);
      mxFree(b);
      mxFree(q);
      mxFree(z);
      return;
   }
   mxFree(etawt1);
   md1 = n-nunstab<pin?n-nunstab:pin;
   bigev1 = md1;
   for (i=0; i<md1; i++)
      if (deta1[i]<=REALSMALL) {
         bigev1=i;
         break;
      }
   /* ueta1 = n-nunstab x bigev1
      deta1 = bigev1 x 1
      veta1 = bigev1 x pin, ldveta1 = md1 */

   if (existx || !nunstab) {
   /* mexPrintf("Solution exists\n"); */
      eu[0] = 1;
   } else {
      if (exist) {
         mexPrintf("Solution exists for unforecastable z only\n");
         eu[0] = -1;
      } /* else
         mexPrintf("No solution.  %d unstable roots. %d endog errors.\n",nunstab,bigev1); */
   /* mexPrintf("Generalized eigenvalues\n");
      mexCallMATLAB(0,NULL,1, &plhs[6], "disp"); */
   }
   if (!bigev1)
      unique = true;
   else {
      /* veta1-veta1*veta*veta' */
      /* veta = bigev x pin, ldveta1 = md
         veta1 = bigev1 x pin, ldveta1 = md1 */
      MKL_Complex16 *tmp = mxCalloc(pin*pin,sizeof(MKL_Complex16));
      MKL_Complex16 *veta1_copy = mxCalloc(pin*bigev1,sizeof(MKL_Complex16));
      cblas_zgemm(CblasColMajor, CblasConjTrans, CblasNoTrans, pin, pin,
         bigev, &one, veta, md, veta, md, &zero, tmp, pin);
      cblas_zdupe(bigev1,pin,veta1,md1,veta1_copy,bigev1);
      cblas_zhemm(CblasColMajor, CblasRight, CblasUpper, bigev1, pin,
         &minusone, tmp, pin, veta1_copy, bigev1, &one, veta1_copy, bigev1);
      mxFree(tmp);
      if (compute_norm(veta1_copy, &norm, bigev1, pin)) {
         mexWarnMsgTxt("SVD failed.\n");
         mxFree(norm);
         mxFree(ueta1);
         mxFree(deta1);
         mxFree(veta1);
         mxFree(ueta);
         mxFree(deta);
         mxFree(veta);
         mxFree(veta1_copy);
         mxFree(a);
         mxFree(b);
         mxFree(q);
         mxFree(z);
         return;
      }
      mxFree(veta1_copy);
      unique = *norm < REALSMALL*n;
      mxFree(norm);
   }
   if (unique) {
   /* mexPrintf("Solution unique\n"); */
      eu[1] = 1;
   } else {
      mexPrintf("Indeterminacy.  %d loose endog errors.\n",bigev1-bigev);
   /* mexPrintf("Generalized eigenvalues\n");
      mexCallMATLAB(0,NULL,1, &plhs[6], "disp"); */
   }

   cblas_zdscali(pin,deta,bigev,veta,md);      /* veta' = deta\veta' */
   mxFree(deta);
   cblas_zdscale(pin,deta1,bigev1,veta1,md1);  /* veta1' = deta1*veta1' */
   mxFree(deta1);
   etawt = mxCalloc(nunstab*pin,sizeof(MKL_Complex16));      /* etawt = ueta*veta' */
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, nunstab, pin, bigev,
      &one, ueta, nunstab, veta, md, &zero, etawt, nunstab);
   mxFree(ueta);
   mxFree(veta);
   etawt1 = mxCalloc((n-nunstab)*pin,sizeof(MKL_Complex16)); /* etawt1 = ueta1*veta1' */
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n-nunstab, pin, bigev1,
      &one, ueta1, n-nunstab, veta1, md1, &zero, etawt1, n-nunstab);
   mxFree(ueta1);
   mxFree(veta1);
   tmat = mxCalloc((n-nunstab)*nunstab,sizeof(MKL_Complex16)); /* tmat = etawt1*etawt' */
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasConjTrans, n-nunstab, nunstab, pin,
      &one, etawt1, n-nunstab, etawt, nunstab, &zero, tmat, n-nunstab);
   mxFree(etawt1);
   mxFree(etawt);

   g0 = mxCalloc(n*n,sizeof(MKL_Complex16));
   cblas_zdupe(n-nunstab, n, a, n, g0, n);
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n-nunstab, nunstab, nunstab,
      &minusone, tmat, n-nunstab, a+(n-nunstab)*(n+1), n, &one, g0+(n-nunstab)*n, n);
   cblas_zcopy(nunstab, &one, 0, g0+(n-nunstab)*(n+1), n+1);

   g1 = mxCalloc(n*n,sizeof(MKL_Complex16));
   cblas_zdupe(n-nunstab, n, b, n, g1, n);
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n-nunstab, nunstab, nunstab,
      &minusone, tmat, n-nunstab, b+(n-nunstab)*(n+1), n, &one, g1+(n-nunstab)*n, n);
   cblas_ztrsm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit,
      n, n, &one, g0, n, g1, n);
   dummy = mxCalloc(n*n,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n, n, n, &one, z, n, g1, n, &zero, dummy, n);
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasConjTrans, n, n, n, &one, dummy, n, z, n, &zero, g1, n);
   mxFree(dummy);
   plhs[0] = mkl2mat_real(g1, n, n, n);
   mxFree(g1);

   tmatq = mxCalloc(n*n,sizeof(MKL_Complex16));
   cblas_zcopy(n*n, q, 1, tmatq, 1);
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasConjTrans, n, n-nunstab, nunstab,
      &minusone, tmatq+(n-nunstab)*n, n, tmat, n-nunstab, &one, tmatq, n);
   mxFree(tmat);

   ab = mxCalloc(nunstab*nunstab,sizeof(MKL_Complex16));
   cblas_zdpsb(nunstab, nunstab, a+(n-nunstab)*(n+1), n, b+(n-nunstab)*(n+1), n, ab, nunstab);
   cblas_ztrsm(CblasColMajor, CblasRight, CblasUpper, CblasConjTrans, CblasNonUnit,
      n, nunstab, &one, ab, nunstab, tmatq+(n-nunstab)*n, n);
   mxFree(ab);

   c = mat2mkl(prhs[2],n,1);
   cblas_zgemv(CblasColMajor, CblasConjTrans, n, n, &one, tmatq, n, c, 1, &zero, c, 1);
   cblas_ztrsv(CblasColMajor, CblasUpper, CblasNoTrans, CblasNonUnit, n, g0, n, c, 1);
   cblas_zgemv(CblasColMajor, CblasNoTrans, n, n, &one, z, n, c, 1, &zero, c, 1);
   plhs[1] = mkl2mat_real(c, n, n, 1);
   mxFree(c);

   impact = mxCalloc(n*psin,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasConjTrans, CblasNoTrans, n-nunstab, psin, n,
      &one, tmatq, n, psi, n, &zero, impact, n);
   mxFree(tmatq);
   cblas_ztrsm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit,
      n, psin, &one, g0, n, impact, n);
   dummy = mxCalloc(n*psin,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n, psin, n,
      &one, z, n, impact, n, &zero, dummy, n);
   mxFree(impact);
   plhs[2] = mkl2mat_real(dummy, n, n, psin);
   mxFree(dummy);

   fmat = a+(n-nunstab)*(n+1);
   cblas_ztrsm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit,
      nunstab, nunstab, &one, b+(n-nunstab)*(n+1), n, fmat, n);
   plhs[3] = mkl2mat(fmat, n, nunstab, nunstab);
   mxFree(a);

   fwt = mxCalloc(nunstab*psin,sizeof(MKL_Complex16));
   cblas_ztrsm(CblasColMajor, CblasRight, CblasUpper, CblasConjTrans, CblasNonUnit,
      n, nunstab, &one, b+(n-nunstab)*(n+1), n, q+(n-nunstab)*n, n);
   mxFree(b);
   cblas_zgemm(CblasColMajor, CblasConjTrans, CblasNoTrans, nunstab, psin, n,
      &minusone, q+(n-nunstab)*n, n, psi, n, &zero, fwt, nunstab);
   mxFree(q);
   mxFree(psi);
   plhs[4] = mkl2mat(fwt, nunstab, nunstab, psin);
   mxFree(fwt);

   ywt = mxCalloc(n*nunstab,sizeof(MKL_Complex16));
   cblas_zcopy(nunstab, &one, 0, ywt+n-nunstab, n+1);
   cblas_ztrsm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit,
      n, nunstab, &one, g0, n, ywt, n);
   mxFree(g0);
   dummy = mxCalloc(n*nunstab,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n, nunstab, n,
      &one, z, n, ywt, n, &zero, dummy, n);
   mxFree(z);
   mxFree(ywt);
   plhs[5] = mkl2mat(dummy, n, n, nunstab);
   mxFree(dummy);
}

/******************************************************************************
 * function selctg orders the eigenvalues so that a selected cluster of       *
 * eigenvalues appears in the leading diagonal blocks of the upper            *
 * quasi-triangular matrix S and the upper triangular matrix T.               *
 ******************************************************************************/

int selctg(MKL_Complex16 *alpha, MKL_Complex16 *beta) {
   double absA = sqrt(alpha->real*alpha->real+alpha->imag*alpha->imag),
          absB = fabs(beta->real);
   if (absA) {
      double divhat = absB/absA;
      if (fixdiv && 1+REALSMALL<divhat && divhat<stake)
         stake = (1+divhat)/2;
   }
   if (absA<REALSMALL && absB<REALSMALL)
      zxz = true;
   if (absB>stake*absA) {
      nunstab++;
      return(0);
   } else
      return(1);
}

/******************************************************************************
 * compute for a pair of N-by-N complex nonsymmetric matrices (A,B),          *
 * the generalized eigenvalues, the generalized complex Schur form (S, T),    *
 * and optionally left and/or right Schur vectors (VSL and VSR)               *
 ******************************************************************************/

int qz(MKL_Complex16 *a, MKL_Complex16 *b, MKL_Complex16 *q, MKL_Complex16 *z, int n) {

   unsigned char msg[101];
   int sdim, lwork = -1, info = 0;
   MKL_Complex16 *alpha = mxCalloc(n,sizeof(MKL_Complex16)),
                  *beta = mxCalloc(n,sizeof(MKL_Complex16)),
                  *work, work1;
   double *rwork = mxCalloc(8*n,sizeof(double));
   bool *bwork = mxCalloc(4*n,sizeof(bool));

   /* Query zgges on the value of lwork */
   zgges("V", "V", "S", &selctg, &n, a, &n, b, &n, &sdim, alpha, beta, q,
         &n, z, &n, &work1, &lwork, rwork, bwork, &info);

   if (info < 0) {
      sprintf(msg, "Input %d to zgges had an illegal value",-info);
      mxFree(bwork);
      mxFree(rwork);
      mxFree(alpha);
      mxFree(beta);
      mexWarnMsgTxt(msg);
      return(info);
   }

   lwork = (int)(work1.real);
   work = mxCalloc(lwork,sizeof(MKL_Complex16));
   zgges("V", "V", "S", &selctg, &n, a, &n, b, &n, &sdim, alpha, beta, q,
         &n, z, &n, work, &lwork, rwork, bwork, &info);

   mxFree(work);
   mxFree(bwork);
   mxFree(rwork);
   mxFree(alpha);
   mxFree(beta);

   if (info < 0) {
      sprintf(msg, "Input %d to zgges had an illegal value",-info);
      mexWarnMsgTxt(msg);
      return(info);
   }

   if (info > 0)
      if (info < n) {
         sprintf(msg,"The QZ iteration failed.  (A,B) are not in Schur form,\n"
             "but ALPHA(j) and BETA(j) should be correct for j=%d,...,N.",info+1);
         mexWarnMsgTxt(msg);
      } else {
         switch (info-n) {
         case 1:
            sprintf(msg,"LAPACK problem: error return from ZGGBAL");
            break;
         case 2:
            sprintf(msg,"LAPACK problem: error return from ZGEQRF");
            break;
         case 3:
            sprintf(msg,"LAPACK problem: error return from ZUNMQR");
            break;
         case 4:
            sprintf(msg,"LAPACK problem: error return from ZUNGQR");
            break;
         case 5:
            sprintf(msg,"LAPACK problem: error return from ZGGHRD");
            break;
         case 6:
            sprintf(msg,"LAPACK problem: error return from ZHGEQZ (other than failed iteration)");
            break;
         case 7:
            sprintf(msg,"LAPACK problem: error return from ZGGBAK (computing VSL)");
            break;
         case 8:
            sprintf(msg,"LAPACK problem: error return from ZGGBAK (computing VSR)");
            break;
         case 9:
            sprintf(msg,"LAPACK problem: error return from ZLASCL (various places)");
            break;
         default:
            sprintf(msg,"LAPACK problem: unknown error.");
            break;
         }
         mexWarnMsgTxt(msg);
      }

   return(info);
}

/*
 * Convert MATLAB complex matrix to MKL complex storage.
 *
 *          Z = mat2mkl(X,ldz,ndz)
 *
 * converts MATLAB's mxArray X to MKL_Complex16 Z(ldz,ndz).
 * The parameters ldz and ndz determine the storage allocated for Z,
 * while mxGetM(X) and mxGetN(X) determine the amount of data copied.
 */

MKL_Complex16* mat2mkl(const mxArray *X, int ldz, int ndz) {
   MKL_Complex16 *Z, *zp;
   int m, n, incz, cmplxflag;
   register int i, j;
   double *xr, *xi;

   Z = mxCalloc(ldz*ndz, sizeof(MKL_Complex16));
   xr = mxGetPr(X);
   xi = mxGetPi(X);
   m =  mxGetM(X);
   n =  mxGetN(X);
   zp = Z;
   incz = ldz-m;
   cmplxflag = (xi != NULL);
   for (j = 0; j < n; j++) {
      if (cmplxflag) {
         for (i = 0; i < m; i++) {
            zp->real = *xr++;
            zp->imag = *xi++;
            zp++;
         }
      } else {
         for (i = 0; i < m; i++) {
            zp->real = *xr++;
            zp++;
         }
      }
      zp += incz;
   }
   return(Z);
}


/*
 * Convert MKL complex storage to MATLAB real and imaginary parts.
 *
 *          X = mkl2mat(Z,ldz,m,n)
 *
 * copies MKL_Complex16 Z to X, producing a complex mxArray
 * with mxGetM(X) = m and mxGetN(X) = n.
 */

mxArray* mkl2mat(MKL_Complex16 *Z, int ldz, int m, int n) {
   int i, j, incz;
   double *xr, *xi;
   MKL_Complex16 *zp;
   mxArray *X;

   X = mxCreateDoubleMatrix(m,n,mxCOMPLEX);
   xr = mxGetPr(X);
   xi = mxGetPi(X);
   zp = Z;
   incz = ldz-m;
   for (j = 0; j < n; j++) {
      for (i = 0; i < m; i++) {
         *xr++ = zp->real;
         *xi++ = zp->imag;
         zp++;
      }
      zp += incz;
   }
   return(X);
}

/*
 * Convert MKL complex storage to MATLAB real matrix ignoring imaginary part.
 *
 *          X = mkl2mat(Z,ldz,m,n)
 *
 * copies MKL_Complex16 Z to X, producing a real mxArray
 * with mxGetM(X) = m and mxGetN(X) = n.
 */

mxArray* mkl2mat_real(MKL_Complex16 *Z, int ldz, int m, int n) {
   int i, j, incz;
   double *xr;
   MKL_Complex16 *zp;
   mxArray *X;

   X = mxCreateDoubleMatrix(m,n,mxREAL);
   xr = mxGetPr(X);
   zp = Z;
   incz = ldz-m;
   for (j = 0; j < n; j++) {
      for (i = 0; i < m; i++) {
         *xr++ = zp->real;
         zp++;
      }
      zp += incz;
   }
   return(X);
}

void copy_eigenvalues(mxArray *gev, MKL_Complex16 *a, MKL_Complex16 *b, int n) {
   double *gevr = mxGetPr(gev),
          *gevi = mxGetPi(gev);
   int i;

   for (i=0; i<n; i++, gevr++, gevi++, a+=n+1) {
      *gevr = a->real;
      *gevi = a->imag;
   }

   for (i=0; i<n; i++, gevr++, gevi++, b+=n+1) {
      *gevr = b->real;
      *gevi = b->imag;
   }
}

int compute_svd(MKL_Complex16 *x, MKL_Complex16 **u, double **d, MKL_Complex16 **v, int m, int n) {
   unsigned char msg[101];
   int md = m<n?m:n, lwork = -1, info = 0;
   MKL_Complex16 *a, *work, work1;
   double *rwork = mxCalloc(5*md>1?5*md:1,sizeof(double));

   a = mxCalloc(m*n,sizeof(MKL_Complex16));
   cblas_zcopy(m*n, x, 1, a, 1);

   *u = mxCalloc(m*md,sizeof(MKL_Complex16));
   *v = mxCalloc(md*n,sizeof(MKL_Complex16));
   *d = mxCalloc(md,sizeof(double));

   /* Query zgges on the value of lwork */
   zgesvd("S", "S", &m, &n, a, &m, *d, *u, &m, *v, &md, &work1, &lwork, rwork, &info);

   if (info < 0) {
      sprintf(msg, "Input %d to zgesvd had an illegal value",-info);
      mxFree(rwork);
      mexWarnMsgTxt(msg);
      return(info);
   }

   lwork = (int)(work1.real);
   work = mxCalloc(lwork,sizeof(MKL_Complex16));
   zgesvd("S", "S", &m, &n, a, &m, *d, *u, &m, *v, &md, work, &lwork, rwork, &info);

   mxFree(work);
   mxFree(rwork);
   mxFree(a);

   if (info < 0) {
      sprintf(msg, "Input %d to zgesvd had an illegal value",-info);
      mexWarnMsgTxt(msg);
   }

   if (info > 0) {
      sprintf(msg,"ZBDSQR did not converge.\n%d superdiagonals of an intermediate "
         "bidiagonal form B did not converge to zero.",info);
      mexWarnMsgTxt(msg);
   }
   return(info);
}

int compute_norm(MKL_Complex16 *a, double **d, int m, int n) {
   unsigned char msg[101];
   int md = m<n?m:n, lwork = -1, info = 0;
   MKL_Complex16 *work, work1;
   double *rwork = mxCalloc(5*md>1?5*md:1,sizeof(double));

   *d = mxCalloc(md,sizeof(double));

   /* Query zgges on the value of lwork */
   zgesvd("N", "N", &m, &n, a, &m, *d, NULL, &m, NULL, &md, &work1, &lwork, rwork, &info);

   if (info < 0) {
      sprintf(msg, "Input %d to zgesvd had an illegal value",-info);
      mxFree(rwork);
      mexWarnMsgTxt(msg);
      return(info);
   }

   lwork = (int)(work1.real);
   work = mxCalloc(lwork,sizeof(MKL_Complex16));
   zgesvd("N", "N", &m, &n, a, &m, *d, NULL, &m, NULL, &md, work, &lwork, rwork, &info);

   mxFree(work);
   mxFree(rwork);

   if (info < 0) {
      sprintf(msg, "Input %d to zgesvd had an illegal value",-info);
      mexWarnMsgTxt(msg);
   }

   if (info > 0) {
      sprintf(msg,"ZBDSQR did not converge.\n%d superdiagonals of an intermediate "
         "bidiagonal form B did not converge to zero.",info);
      mexWarnMsgTxt(msg);
   }
   return(info);
}

int compute_normx(MKL_Complex16 *a, MKL_Complex16 *b, MKL_Complex16 *zwt, MKL_Complex16 *ueta, double **normx, int nunstab, int psin, int n, int bigev) {
   int info = 0, i, bigevs;
   MKL_Complex16 *M, *zwtx, *ux, *vx, *tmp;
   double *dx;

   a += (n+1)*(n-nunstab);
   b += (n+1)*(n-nunstab);
   cblas_ztrsm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit,
      nunstab, psin, &one, b, n, zwt, nunstab);
   M = mxCalloc(nunstab*nunstab,sizeof(MKL_Complex16));
   cblas_zdupe(nunstab, nunstab, a, n, M, nunstab);
   cblas_ztrsm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit,
      nunstab, nunstab, &one, b, n, M, nunstab);

   zwtx = mxCalloc(nunstab*nunstab*psin,sizeof(MKL_Complex16));
   cblas_zcopy(nunstab*psin, zwt, 1, zwtx, 1);
   for (i=1; i<nunstab; i++) {
      cblas_ztrmm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, nunstab, psin*i, &one, M, nunstab, zwtx, nunstab);
      cblas_zcopy(nunstab*psin, zwt, 1, zwtx+nunstab*psin*i, 1);
   }
   mxFree(M);
   cblas_ztrmm(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, nunstab, nunstab*psin, &one, b, n, zwtx, nunstab);
   info = compute_svd(zwtx, &ux, &dx, &vx, nunstab, nunstab*psin);
   mxFree(vx);
   mxFree(zwtx);
   if (info) {
      mexWarnMsgTxt("SVD failed.\n");
      mxFree(ux);
      mxFree(dx);
      return(info);
   }
   bigevs = nunstab;
   for (i=0; i<nunstab; i++)
      if (dx[i]<=REALSMALL) {
         bigevs = i;
         break;
      }
   mxFree(dx);
   /* ux-ueta*ueta'*ux */
   tmp = mxCalloc(nunstab*nunstab,sizeof(MKL_Complex16));
   cblas_zgemm(CblasColMajor, CblasNoTrans, CblasConjTrans, nunstab, nunstab,
      bigev, &one, ueta, nunstab, ueta, nunstab, &zero, tmp, nunstab);
   cblas_zhemm(CblasColMajor, CblasLeft, CblasUpper, nunstab,
      bigevs, &minusone, tmp, nunstab, ux, nunstab, &one, ux, nunstab);
   mxFree(tmp);
   info = compute_norm(ux, normx, nunstab, bigevs);
   if (info) {
      mexWarnMsgTxt("SVD failed.\n");
      mxFree(normx);
      mxFree(ux);
      return(info);
   }
   mxFree(ux);
   return(info);
}

void cblas_zdupe(int m, int n, MKL_Complex16 *a, int lda, MKL_Complex16 *b, int ldb) {
   register int i;
   for (i=0; i<m; i++, a++, b++)
      cblas_zcopy(n, a, lda, b, ldb);
}

void cblas_zdscali(int n, double *a, int lda, MKL_Complex16 *b, int ldb) {
   register int i;
   for (i=0; i<lda; i++, a++, b++)
      cblas_zdscal(n, 1.0/(*a), b, ldb);
}

void cblas_zdscale(int n, double *a, int lda, MKL_Complex16 *b, int ldb) {
   register int i;
   for (i=0; i<lda; i++, a++, b++)
      cblas_zdscal(n, *a, b, ldb);
}

void cblas_zdpsb(int m, int n, MKL_Complex16 *a, int lda, MKL_Complex16 *b, int ldb, MKL_Complex16 *c, int ldc) {
   register int i;
   cblas_zdupe(m, n, a, lda, c, ldc);
   for (i=0; i<m; i++, b++, c++)
      cblas_zaxpy(n, &minusone, b, ldb, c, ldc);
}